Passivity-based Pose Synchronization in Three

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

1

Passivity-based Pose Synchronization in Three Dimensions Takeshi Hatanaka, Member, IEEE, Yuji Igarashi, Masayuki Fujita, Member, IEEE and Mark W. Spong, Fellow, IEEE

Abstract—This paper addresses passivity-based pose synchronization in the Special Euclidean group SE(3). We first develop a passivity-based distributed velocity input law to achieve pose synchronization. We next show that the pose synchronizes exponentially fast under an assumption on the initial states of the bodies, with an exponential convergence rate given by algebraic connectivity of interconnection graphs. We also prove pose synchronization in the presence of communication delays and topology switches. We moreover give further extensions of the present law, where desirable velocities and collision avoidance are taken into account. Finally, the effectiveness of the present inputs is demonstrated through numerical simulations and experiments on a planar (2D) test bed. Index Terms—Cooperative systems, Synchronization,

xi

zi

Oi

z

yi gi Frame i gi gj

zj xj

y O x

gj

World Frame Fig. 1.

Oj Frame

yj j

Rigid-Body Motion in SE(3)

I. INTRODUCTION Cooperative control have received a lot of attention with numerous practical applications, such as robotic networks [1], mobile sensor networks [2], [3], formation control [4] and vehicle networks. In addition, cooperative control is also motivated by scientific interest in cooperative behavior in nature, such as flocking of birds [5], [6] and schooling of fish [7]. The objective in cooperative control problems is to design a distributed control strategy using only local information so that the aggregate system achieves specified behaviors, such as consensus [8], [9], flocking [5]–[14], synchronization [15]– [17], coordination [18]–[23] and coverage [24], [25]. Passivity and passivity-based control have proved useful for the problem of motion coordination of multi-agent systems [17]–[28]. In [17], [26] passivity-based control laws are presented for output synchronization of a group of passive nonlinear systems. As shown in these references, passivitybased control enables one to handle communication delays and switching topology within a unified (energy-based) framework. Motion coordination in non-Euclidean manifolds is also gaining increasing interest [18]–[23], [29]–[37]. Sepulchre et al. [20], [21] consider a model of identical planar particles whose phase variable evolves on the circle S 1 , and present steering control laws to achieve some different types of motion Takeshi Hatanaka(corresponding author) and Masayuki Fujita are with the Department of Mechanical and Control Engineering, Tokyo Institute of Technology, Tokyo 152-8550, JAPAN, [email protected], [email protected], [email protected] Yuji Igarashi is with the Advanced Technology R&D Center, Mitsubishi Electric Cooperation, Amagasaki, Hyogo 661-8661, Japan, [email protected] M.W. Spong is with the Erik Jonsson School of Engineering and Computer Science at the University of Texas at Dallas, Dallas, TX, 75080-3021, USA, [email protected]

coordination in SE(2) for both all-to-all and limited communication. Motion coordination on SE(3) with steering control is investigated in [18], [22], [23], where it is shown that the group of particles forms either of three types of coordinated motions (parallel, circular and helical motions) in the relative equiribria of the particle models. Attitude synchronization (or motion synchronization in SO(3)) is tackled in [19],[29]–[37], where [19],[29]–[32] consider multiple rigid bodies with attitude dynamics represented by Euler-Lagrange equations, [33] uses a kinematic model, and [36], [37] handles general Lagrangian systems. [19] presents autonomous attitude synchronization laws achieving local synchronization, which relies only on the relative poses. A scheme based on consensus on a vector space is proposed in [31] to assure almost globally asymptotic synchronization. [36], [37] present globally exponential synchronization laws [38], [39] in the presence of the desirable trajectories of the group. [29], [30] present attitude coordination laws by using the inertial frame information. Passivitybased control laws are presented in [32], [33]. In one of our previous works [33], exponential attitude synchronization and convergence in the presence of the communication failures and communication delays are proved. However, the group would break apart without mutual feedbacks on positions due to the effects on some disturbed force in practice. We thus present a unified framework to deal with both of position and attitude synchronization based on passivity. In this paper, we address pose synchronization based on some techniques developed in [26], [33], [34], [40], [41]. We first present a pose synchronization control law based on the fact that the kinematics of a rigid body in SE(3) has a passivity-like property. Then, we show that the pose synchronizes by using the proposed velocity input under milder


assumptions than those in [33], [34]. It is also proved that pose synchronization is still achieved in the presence of communication delays. We next show that the pose synchronizes exponentially fast under an assumption on the initial states of the bodies, with an exponential convergence rate given by so-called algebraic connectivity [8], [9] of the interconnection graph. The result partially explains the relation between the convergence speed and the graph structures in a local region. Using the exponential convergence result and the notion of brief connectivity, we then investigate temporary communication failures. Next, we give some extensions to the case with a desirable velocity of the group and a flocking algorithm in SE(3) containing collision avoidance force. We finally demonstrate the validity of the results through simulations and experiments on a planar testbed. The main contributions of this paper is listed below. (i) Only strong connectivity on the graph is assumed to prove synchronization while other energy-based works require strongly connected balanced graphs or connected undirected graphs. (ii) Synchronization is proved even in the presense of a class of communication delays. (iii) It is shown that when the present law is applied, not only asymptotic but also exponential synchronization are achieved under some assumptions of the initial states without using any additional information other than the relative poses. (iv) Not only numerical simulation but also experiments are performed in order to demonstrate the validity of our control law. In the previous works, the items (i)–(iii) are reached only by assuming helps of communication [31] or inertial frame information [36], [37], but, thanks to this paper, it is shown that the issues can be handled without such helps, which expands usability of synchronization schemes. This paper is organized as follows. Section II formulates rigid-body motion in SE(3) and the graph structure considered in this paper. We show that the rigid-body motion in SE(3) is passive and introduce the pose synchronization problem. In Section III, we present a body velocity control law based on the passivity and prove pose synchronization. In Section IV, we prove exponential synchronization with a convergence rate given by the algebraic connectivity and investigate topology switches. In Section V, we give further extensions of the present control law. We demonstrate our results through numerical simulations in Section VI and through experiments on a planar (2D) test bed in Section VII. We finally present conclusions in Section VIII. II. P ROBLEM S TATEMENT A. System Description Throughout this paper, we consider the motion of a group of n rigid bodies in 3-dimensional space (see Figure 1). Let Σw be an inertial coordinate frame and Σi , i ∈ {1, · · · , n} a bodyfixed coordinate frame whose origin is located at the center of mass of body i. We assume that all the coordinate frames are right-handed and Cartesian and we denote by pi ∈ R3 the position of the rigid body i ∈ {1, · · · , n} in a fixed inertial ˆ coordinate frame Σw . We will use eξi θi ∈ R3×3 to represent the rotation matrix of a body-fixed frame Σi relative to an inertial coordinate frame Σw . Here, ξi ∈ R3 , ξiT ξi = 1 and

2

θi ∈ R specify the direction of rotation and the angle of rotation, respectively. The notation ‘∧’ (wedge) is the skewsymmetric operator from R3 to the space of 3 × 3 skewsymmetric matrices, namely  ∧   ξ1 0 −ξ3 ξ2 ξ2  =  ξ3 0 −ξ1  . ξ3 −ξ2 ξ1 0 The notation ‘∨’ (vee) denotes the inverse operator to ‘∧’. ˆ The transformation eξi θi is orthogonal with unit determinant i.e. an element of the Special Orthogonal group SO(3). A ˆ configuration consists of the pair (pi , eξi θi ) and hence the configuration space of the rigid-body motion is the Special Euclidean group SE(3), which is the product space of R3 with SO(3). We use the 4 × 4 matrix [ ˆ ] eξi θi pi gi = , i ∈ {1, · · · , n} 0 1 ˆ

as the homogeneous representation of (pi , eξi θi ) ∈ SE(3). Let us now introduce the velocity of each rigid body to represent the rigid-body motion of the frame Σi relative to Σw . Define the body velocity Vib := (vi , ωi ) and [ ] ω ˆ vi Vîb = i , i ∈ {1, · · · , n}, 0 0 where vi ∈ R3 and ωi ∈ R3 are the linear and angular velocities of body i relative to Σi respectively. Then, each rigid-body motion is represented by the kinematic model g˙ i yi

= [ gi Vîb , ˆ eξi θi = 0

] qi , i ∈ {1, · · · , n}. 1

(1)

where yi ∈ SE(3) is a controlled output, qi := pi + di is a virtual position and di ∈ R3 is a bias of body i. The main advantages to using the above homogeneous representation are global and geometric descriptions of rigid-body motion, which greatly simplifies the analysis in 3-dimensionals. For more details on the rigid-body motion in SE(3), refer to [42], [43]. The interconnection of a network of rigid bodies is represented by a weighted digraph GO = (V, EO , W), where V := {1, · · · , n} is the node set, EO ⊂ V × V is the edge set containing pairs of nodes that represent communication and W is the weight set. The neighbors of body i are defined as NOi := {j ∈ V | (j, i) ∈ EO } [8] , which means that agent i receives information from agent j if j ∈ NOi . We moreover denote by Lw the weighted graph Laplacian matrix (see e.g. [9]) of the graph GO B. Passivity-like Property in SE(3) In this section, we show that the kinematic model (1) possess a passivity-like property. For this purpose, we define the total energy of translation and rotation ] [1 0 2 2 I3 ψ(yi ) := ∥J(I4 − yi )∥F , J := √1 0 2 1 1 ˆ 2 ξî θi ξî θi ∥qi ∥ + ϕ(e ), ϕ(e ) := tr(I3 − eξi θi ) = 2 2


3

where In is the n × n identity matrix, ∥ · ∥F represents the Frobenius matrix norm (∥A∥F = tr(AT A)1/2 ) and ∥ · ∥ the Euclidean vector norm. By the definition, ψ(yi ) = 0 if and only if yi = I4 . Lemma 1: The time derivative of ψ(yi ) along the trajectories of (1) satisfies [ ] [ ] ˆ vi e−ξi θi qi b T b ˙ ψ(yi ) = (Vi ) Πi , Vi = , Πi := , (2) ˆ ωi sk(eξi θi )∨ ˆ

ˆ

where sk(eξi θi ) is the skew-symmetric part of the matrix eξi θi , ˆ ˆ ˆ i.e. sk(eξi θi ) := 21 (eξi θi − e−ξi θi ). Proof: Immediate from ([44, pp. 42 Lemma 1]). If we now consider the velocity Vib as an input and the vector form of the rigid-body motion Πi as an output, Lemma 1 says that the rigid-body motion in SE(3) (1) is passive from the input Vib to the output Πi (This property is called a passivitylike property throughout this paper) in the sense defined in [47], since integrating (2) from 0 to T yields ∫ T (Vib )T Πi dt = ψ(yi (T )) − ψ(yi (0)) ≥ −ψ(yi (0)). 0

Throughout this paper we consider the kinematic model (1) and the velocity Vib as a control input. In contrast, some practical mechanical systems such as spacecraft or UAV systems use torque and force control. Even in such real systems, considering simplified dynamics can be useful at least to build a high-level planning controller generating desired trajectories under the assumption that these can be tracked by a lower level mechanical controller. In addition, it is also useful as a preliminary step towards an integrated controller. C. Pose Synchronization in SE(3) In this section, we investigate pose synchronization defined as follows. Definition 1 (Pose Synchronization): A group of n rigid bodies is said to achieve pose synchronization, if lim ψ(yi−1 yj ) = 0 ∀i, j(i ̸= j) ∈ {1, · · · , n}.

t→∞

j

j

i

i l

w Fig. 2.

(3)

i

l

Pose Synchronization in SE(3)

By the definition of the function ψ, equation (3) implies the virtual positions and orientations of all the rigid bodies converge to a common value. From the definition of the virtual position, it means that relative positions between rigid bodies converge to desired ones di − dj ∀i, j (Figure 2). If di = 0, then yi = gi and pose synchronization defined above coincides with the precise meaning of the word, which is partially investigated in [34]. Note that the convergence of only the orientations is called attitude synchronization [32], [33], [31].

III. C ONVERGENCE AND C ONNECTIVITY A NALYSIS A. Velocity Control Law and Convergence The goal of this section is to design a body velocity input Vib so that the group of rigid bodies achieves pose synchronization. We first propose the body velocity input ]) ∑ ( [e−ξî θi 0][ qi − qj b Vi = −Ki wij , (4) ˆ ˆ 0 I sk(e−ξj θj eξi θi )∨ j∈NOi

for all i ∈ {1, · · · , n} based property [ on the passivity-like ] kpi I3 0 (Lemma 1), where Ki = , kpi > 0, kei > 0. 0 kei I3 Note that the present control law (4) feedbacks relative information on virtual positions and orientations with respect to the neighbors instead of the self information. These variables can be obtained by the visual measurement even without communication as shown in [35]. Let us now explain an interpretation of the control input (4). (4) can be rewritten as [ ] ( ∑ ) e−ξî θi (q − ⟨q ⟩) i i b Vi = −Ki wij ∀i, (5) ˆ ˆ sk(⟨e−ξi θi ⟩eξi θi )∨ j∈N Oi

ˆ

where ⟨qi ⟩ and ⟨eξj θj ⟩ are weighted average position and orientation of neighbors of rigid body i, i.e., ∑ ∑ ˆ ⟨qi ⟩ := ( j∈NOi wij qj )/( j∈NOi wij ) and ⟨e−ξi θi ⟩ := ∑ ∑ ˆ ( j∈NOi wij e−ξj θj )/( j∈NOi wij ), respectively. We see from (5) that each rigid body is steered toward the weighted center of positions and orientations with respect to the neighbors. By using the input (4), we get the following theorem. Theorem 1: Consider n rigid bodies represented by (1). ˆ Then, under the assumption that there exists eξα θα such that ˆ ˆ ˆ e¯ξi θi := e−ξα θα eξi θi ∀i are positive definite1 at the initial time and the interconnection graph GO is fixed and strongly connected, the velocity input (4) achieves pose synchronization in the sense of (3). Proof: See Appendix A-B. The condition on the orientation matrices is milder than that in ˆ [33], [34]. The existence of eξα θα satisfying the condition of Theorem 1 means that there is a coordinate transformation of the world frame such that all rigid bodies’ orientation matrices become positive definite. Since the orientation matrices are positive definite if and only if the rotation angles are less than π/2, such a transformation makes all rigid bodies’ angles ˆ become less than π/2. Suppose that eξα θα is viewed as an orientation of a static leader (numbered 1 without loss of generality) and its position is set arbitrarily. Then, we can immediately prove by using the same potential function UO as Theorem 1 that pose synchronization, i.e., limt→∞ ψ(y1T yi ) = 0 ∀i, is also achieved for a graph with a root node under the ˆ ˆ conditions: 1) the relative orientation matrices e−ξ1 θ1 eξi θi are positive definite. 2) The interconnection graph excluding body 1 Throughout this paper, we refer to a real matrix M , which is not necessarily symmetric, as a positive definite (positive semi-definite) matrix if and only if xT M x > 0 (xT M x ≥ 0) for all nonzero vector x.


4

1 is fixed and strongly connected. 3) There exists at least one i satisfying 1 ∈ Ni . Theorem 1 gives a sufficient condition for pose synchronization when the present control input is applied to each body similarly to the other related works [19]–[21], [29], [30], [32]–[37]. The conditions are necessary in our procedure of the proof in order to define the energy function UO satisfying (27) and to assure that the right-hand side of (21) is not positive. However, we can numerically confirm the existence of initial states and graph structures achieving synchronization even if the assumptions are not satisfied. A necessary and sufficient condition for pose synchronization in the framework is currently open, and hence we mention only strength of the condition relative to the other works below. In the proof of Theorem 1, the potential function UO is defined as a weighted sum of the energy functions ψ(yi ) and used as a Lyapunov function candidate. This choice is quite natural from the viewpoint that the kinematic model (4) is passive (Lemma 1). In addition, this potential function enables us to remove the balanced graph assumption of earlier approaches, e.g. [26], and thus prove pose synchronization for a wider class of information graphs, namely, strongly connected graphs. To the best of our knowledge, the assumption in Theorem 1 is the mildest at least among all of the other works in the energybased framework including consensus in Rn and the other synchronization laws based only on relative information. It should be however noted that if feedbacks of more information other than relative poses via communication are assumed, there are some works [23], [31] achieving synchronization or coordination under a milder assumption that the information graph has a directed spanning tree or is jointly connected. This paper currently considers only local behaviors similarly to [19], [12], [17], [45], [46]. Meanwhile, there are several works investigating global behaviors in motion coordination problems on non-Euclidean space [18], [20]–[23], [29]–[37]. However, it is challenging to achieve global synchronization by using only relative poses as stated in [32], which is the only exceptional paper and presents a control law assuring almost global convergence for acyclic graphs without using any additional information. B. Communication Delays In this subsection, we consider pose synchronization in the presence of communication delays, which is important in practical applications. We assume that the delay is time invariant and finite, which is the same situation as [26]. In such a case, the pose synchronization is redefined as lim ψ(yj (t − Tji )−1 yi (t)) = 0, ∀i, j j ̸= i,

t→∞

(6)

where Tij ≥ 0 is the summation of delays in the communication from rigid body i to rigid body j. Accordingly, we modify the input (4) as [ ] ˆ ∑ e−ξi θi (qj (t − Tji ) − qi (t)) b (7) Vi = Ki wij , ˆ ˆ sk(e−ξi θi (t) eξj θj (t−Tji ) )∨ j∈N Oi

for all i ∈ {1, · · · , n} based on [26]. Then, we have the following corollary.

Corollary 1: Suppose that there exist n rigid bodies represented by (1). Then, under the assumption that there exists ˆ ˆ ˆ eξα θα such that e−ξα θα eξi θi ∀i are positive definite at the initial time and the interconnection graph GO is fixed and strongly connected, the velocity input (7) achieves pose synchronization in the sense of (6). Proof: This corollary is proved in the same way as Theorem 1 by using the following potential function, n n ∑ ∑ ∫ t ∑ γi wij Ui (τ )dτ, (8) Udelay := γi Ui (t)+ i=1

i=1 j∈NOi

t−Tji

where Ui :=

1 1 ˆ ∥qi ∥2 + ϕ(¯ eξi θi ), i ∈ {1, · · · , n}. 2kpi kei

Remark that the control input (7) does not consist of relative information with respect to neighbors and all the neighbors have to communicate to body i their own attitude matrices with respect to the inertial frame, which is different than the other part of this paper. This implies that all the bodies should share the same inertial frame in contrast to the delay free case. After pose synchronization in the sense of (6) is achieved, the velocity input (7) is equal to zero and yi becomes constant. In order to meet (6) under the situation, the group also has to satisfy the original definition of synchronization (3). Hence, (7) also achieves pose synchronization in the sense of (3). IV. E XPONENTIAL S YNCHRONIZATION AND S WITCHING T OPOLOGY A. Exponential Synchronization and Convergence Rate The objective of this subsection is to show that the pose synchronizes exponentially fast when the present input (4) is applied to each body and that the convergence rate is given by algebraic connectivity of the graph. For this purpose, we first define an index Uexp evaluating the speed of convergence. In order to measure the speed, Uexp should satisfy the following two conditions: 1) pose synchronization is achieved if and only if limt→∞ Uexp = 0 holds, 2) Uexp (0) is independent of the graph. As such an index, we introduce the function ( ) ˆ ˆ Uexp (t) := Q(t)T(M ⊗I3 )Q(t)+tr (eξθ(t) )T(M ⊗I3 )eξθ(t) . Here, M denotes the graph Laplacian of the non-weighted complete graph (i.e. wij = 1 ∀i, j), namely M := nIn − 11T , where 1 := [1, · · · , 1]T , Q := [q1T , · · · , qnT ]T and ˆ ˆ ˆ eξθ(t) := [e−ξ1 θ1 (t) , · · · , e−ξn θn (t) ]T . This function evaluates the relative poses for all rigid bodies regardless of the actual connectivity, and satisfies both of the above conditions. Note that the function UO clearly does not satisfy the condition 1) and it is necessary to prepare a different function. Another immediate candidate is a function of the output errors between rigid bodies. Such a function can be easily constructed by using the graph Laplacian Lw . However, the use of Lw is prohibited due to the condition 2).


5

Before stating the main result of this section, we introduce the following notation. The notation diag(γ1 , · · · , γn ) represents the diagonal matrix with diagonal elements γ1 , · · · , γn and Lγ denotes Lγ := diag(γ1 , · · · , γn )Lw , where γ T Lw = 0, γ := [γ1 , · · · , γn ]T and ∥γ∥ = 1. Moreover Lγsym := 1 T 2 (Lγ + Lγ ) denotes the symmetric part of the matrix Lγ . Let λmin (L) and λmin2 (L) be the smallest eigenvalue and the second smallest eigenvalue, respectively, of the real symmetric square matrix L. We can now state the following theorem. Theorem 2: Consider the n rigid bodies represented by (1) and the input (4). Then, under the assumption that the ˆ ˆ relative orientations, e−ξi θi eξj θj ∀i, j, are positive definite at the initial time and the interconnection graph GO is strongly connected, there exist positive real numbers a and b such that Uexp (t) ≤ aUexp (0)e−λmin2 (Lγsym )bt .

(9)

In addition, if there exist wl and wu (wu ≥ wl > 0) such that (i, j) ∈ E → wij ∈ [wl , wu ],

(10)

there exist positive real numbers a and b independent of the graph structures satisfying (9). Proof: Appendix B In the inequality (9), the error of poses between rigid bodies exponentially converges to 0, though Theorem 1 only shows asymptotic convergence. Notice now that Theorem 1 assumes the positive definiteness of the individual orientations, while Theorem 2 assumes that of the relative orientations, which is necessary in our procedure of the proof to assure that b > 0 (or ϵ > 0) in (48). An assumption similar to Theorem 2 is made in [16], where exponential synchronization is proved. Now, we can confirm that the assumption in Theorem 2 is more restricˆ ˆ tive than that of Theorem 1 since if e−ξi θi eξj θj > 0 ∀i, j, ξî θi −ξˆα θα ξî θi then e¯ = e e ∀i become positive definite by ξˆα θα ξˆ1 θ1 choosing e =e . From the above discussions, Theorem 2 also guarantees a type of local exponential convergence even with the initial conditions of Theorem 1 since, eventually, the relative orientations will converge to value less than π2 , at which time the remaining convergence will be exponential. More importantly, the latter half of this theorem and the independence of Uexp (0) on the graph structure implies that λmin2 (Lγsym ), which is called algebraic connectivity of a graph, is a metric of the speed of convergence when the present control input (4) is applied. It is well-known in consensus [8] that the algebraic connectivity is a measure of the speed of convergence, and Theorem 2 proves that the same statement is valid at least in a local region for synchronization on SE(3). However, Theorem 2 gives only a sufficient condition for the statement and how global the statement is valid is left as an open problem. It is also difficult to estimate the convergence speed if there is no information on the initial states of the bodies. In such a case, this theorem gives an insight only on the eventual group behavior as mentioned above. The important point of Theorem 2 is that exponential synchronization is proved without using any information other than relative poses, while it is achieved only in the presence of additional information such as the reference trajectories of the group in literature [19], [36], [37]. Introducing a novel index Uexp makes the proof possible.

X(s (t))

1 T 0 Fig. 3.

tl

t) tl +1

t

Brief Connectivity Loss

B. Switching Topology We next investigate the situation where the information graph changes over time. To study the effect of switching topology we utilize the concept of brief instability developed in [48]. This concept will be instrumental in capturing the fraction of the time that the graph may remain disconnected. Let G be a certain set of possible state independent graphs with n nodes and let s(t) : [0, ∞] → G be the piecewise constant switching signal with consecutive switching times separated by a dwell time τD > 0. Namely, any two consecutive switching times tl and tl+1 , l ∈ {0, 1, 2, · · · } satisfy tl+1 − tl ≥ τD , l ≥ 0. The signal s(t) belongs to either of the following subsets of G, 1) Gc ⊆ G: a subset of strongly connected graphs, 2) Gdc ⊆ G: a subset of not strongly connected graphs. It is obvious from the definitions that G = Gc ∪ Gdc holds true. Let us now introduce the connectivity loss time T (τ, t), which is the length of the time when the graph belongs to Gdc over any time interval [τ, t] (Figure 3). The function T (τ, t) is clearly given by { ∫ t 0 s(t) ∈ Gc T (τ, t) = X (s(r))dr, X (s(t)) := . 1 s(t) ∈ Gdc τ Brief connectivity loss [48] means that T (τ, t) ≤ α(t − τ ) + T0 ∀t ≥ τ ≥ 0

(11)

holds for some T0 ≥ 0 and 0 ≤ α < 1. Theorem 3: Consider the n rigid bodies represented by (1) and the input (4). Assume that the relative orientations, ˆ ˆ e−ξi θi eξj θj ∀i, j, are positive definite at the initial time. Then, if the inequality (11) holds, there exists a lower bound of τD such that the velocity input (4) achieves pose synchronization in the sense of (3). Proof: See Appendix C The existence of a lower bound of τD assures that the graph does not switch frequently. Note that we use the result of Theorem 2 in the proof of Theorem 3, which implies that the assumptions of Theorem 3 also give a sufficient condition for synchronization. This is why we assume the positive definiteness of not individual but relative orientations. Since the proof of Lemma 4 is independent of the graph, we need ˆ ˆ to assume the positive definiteness of e−ξi θi eξj θj ∀i, j only at the initial time. In previous works, various approaches are adopted to deal with switching topology, for example, in [10], [11], [14], [26], and [49] where the concepts of joint connectedness,


nonsmooth analysis, dwell time and uniform connectedness are all employed. [23], [31] also achieve motion coordination in SO(3) or SE(3) by using the consensus estimator under the assumption of joint connectivity, which is milder than ours. However, the approach to tackle the problem is different from ours and the comparisons between the approaches have already been shown in Section III.

6

to know the leader’s velocity, although pose synchronization for a static leader is proved under a much milder assumption as stated in Subsection III-A. It is easily confirmed numerically that the poses indeed do not synchronize in case the velocity (vd , ωd ) not equal to 0 is available for a part of followers. This fact indicates that we need to modify the control input itself in order to achieve synchronization under a milder assumption, which is left as a future work of this paper.

V. E XTENSIONS A. Pose Synchronization with Desirable Velocities Just achieving pose synchronization might be insufficient for robotic and vehicle networks where a desirable velocity of the group should be specified. Indeed, if the control input (4) is applied to each body, the group stops i.e. Vib = 0 ∀i after the pose synchronizes. In this subsection, we thus introduce desired linear and) angular velocities denoted by vd and ( ˆ

ˆ

∨

d ξd θd −ξd θd ωd := dt e e respectively. The functions vd and ωd can be time-varying but should be common among all the rigid bodies. Note that, with the knowledge on vd and ωd , the problem would be close to leader-following rather than pose synchronization. The objective here is to present a control law achieving pose synchronization and velocity matching ] [ ] [ [ ] ˆ ξˆj θj e 0 eξi θi 0 v b b Vi = Vj = d ∀i, j. (12) ξˆj θj ξî θi ω d 0 e 0 e

simultaneously. For this purpose, we propose the velocity input ( [ ]) [ ] ˆ ˆ ∑ e−ξi θi vd e−ξi θi (qi − qj ) b Vi = −Ki (13) wij + −ξˆ θ ˆ ˆ e i i ωd sk(e−ξj θj eξi θi )∨ j∈N Oi

for all i ∈ {1, · · · , n}. From the definition of pose synchronization, we immediately see that after the pose synchronizes all rigid bodies have the same linear and angular velocities satisfying (12). For the input (13), we immediately have the following ˆ ˆ ˆ ˆ Corollary, where we redefine e¯ξi θi by e−ξd θd e−ξα θα eξi θi and we use the definition throughout this section. Corollary 2: Consider the n rigid bodies represented by (1) and suppose that vd and ωd represent desired group ˆ trajectories. Then, under the assumption that there exists eξα θα ˆ such that e¯ξi θi ∀i are positive definite at the initial time and the interconnection graph GO is fixed and strongly connected, the velocity input (13) achieves pose synchronization in the sense of (3) and velocity matching (12). Proof: The corollary is proved by just replacing ∫ t qi and ˆ e¯ξi θi in the potential functions UO by q¯i := qi − 0 vd dt and ˆ newly redefined e¯ξi θi respectively. Though we omit the proofs, the other results in the previous sections are also true for (13). The proofs of Theorems 2 and 3 are the same since the additional term is cancelled in (34) and (29). Corollary 1 is also proved by modifications similar to Corollary 2. However, unlike (7), (13) does not always guarantee pose synchronization in the sense of (3) since the bodies continue to move even after synchronization. If the above problem is regarded as a leader-following problem, the control input (13) assumes that all the followers have

B. Flocking Algorithm in Three Dimensions In this subsection, we present a flocking algorithm in three dimensions. Flocking has been considered by a number of researchers, e.g. [5], [6], [10], [11], [12], [13], [14]. In [5], Reynolds introduced three rules to achieve flocking, namely, cohesion, separation(collision avoidance) and alignment rules. Vicsek et al. [6] claimed that a discrete-time model of n agents with the same speed but with different headings achieves flocking. Its theoretical investigations were presented by Jadbabaie et al. [10], where some conditions for achieving flocking were presented. Olfati-Saber [13] also proposed the input with all of the three rules presented by Reynolds. We first state a relation of the input (13) to the original flocking algorithm by Reynolds [5]. As stated in Section III, each body moves toward a weighted center of the neighbor positions, which can be interpreted as the cohesion rule introduced by Reynolds. In addition, the input (13) achieves attitude synchronization and the velocity matching (12) and hence the alignment rule is already incorporated into the input. However, (13) does not embody the third rule i.e. separation. In this section, we thus incorporate the third rule into the input (13) based on the method in [40] and [41], which is also useful in the practical use of the present control law. Though the methodology is in itself not new, we show that the present law is compatible with it. We reformulate the problem in Section III as follows. Rigid body i and j are said to collide iff ∥pi − pj ∥ ≤ r, r > 0. Accordingly, we define the collisional region [40], [41] ∪ Ω := Ωij , Ωij := {P : P ∈ R3n , ∥pi −pj ∥ ≤ r}, j>i

and the sensing region ∪ D := Dij , Dij := {P : P ∈ R3n , r < ∥pi −pj ∥ ≤ R} j>i

where P := [pT1 , · · · , pTn ]T . By definition, P (t) ∈ / Ω means that collisions do not occur between rigid bodies at time t. Additionally, we define a sensing network represented by a position dependent graph GC = (V, EC ), where EC := {(j, i) ∈ V×V | r < ∥pi −pj ∥ ≤ R}. Neighbors of rigid body i with respect to GC is defined as NCi := {j ∈ V | (j, i) ∈ EC }. Throughout this section, we assume ∥di − dj ∥ > r ∀i, j and that each rigid body can get information about rigid body j, if j ∈ NOi ∪ NCi . In the following we propose a body velocity input to guarantee collision avoidance. For this purpose, we use the following functions [40]. ( { })2 ∥pi − pj ∥2 − R2 Uij (pi , pj ) = min 0, , i ̸= j, ∥pi − pj ∥2 − r2


i

Fig. 4.

VI. N UMERICAL S IMULATIONS In this section, we demonstrate the effectiveness of our results by numerical simulation. In our simulation we assume

Rigid body in simulation w12=0.1 w23=0.2 w34=0.3 w45=0.4 1

2

3

4

5

w21=0.2 w51=0.5

Fig. 5.

Graph Topology in Simulation 1

6

4 3

1 0

(pi)2[m]

(pi)1[m]

2

1 2 3 4 5

5

2 1

−1 1 2 3 4 5

−2

0 −3

−1 −2 0

20

40

time[s]

60

−4 0

80

6

20

40

time[s]

60

80

0.6 0.4

4

0.2

2 0 1 2 3 4 5

−2 −4 0

20

40

time[s]

60

0.8

0.6 0.5 0.4 0.3 0.2

0 −0.2 1 2 3 4 5

−0.4 −0.6 −0.8 0

80 1 2 3 4 5

0.7

(ξiθi)1[rad]

Proof: See Appendix D Theorem 4 is proved by using techniques in the proof of Theorem 1 and hence also gives only a sufficient condition for the statement. Note that Theorem 4 assumes the graph GO is undirected while Theorem 1 assumes only strong connectivity. A similar assumption appears in [11], where collision avoidance is studied. In Theorem 4, we prove collision avoidance and attitude synchronization but the rigid-body positions do not always synchronize. Just to achieve collision avoidance and attitude synchronization, it is sufficient to apply some attitude synchronization law and zero linear velocity, namely, collision avoidance is important only when positions must move. However, the algorithm is different from the flocking algorithm of Reynolds since cohesion is not incorporated into the input. Thus, the important point here is that these properties are achieved even in the presence of the cohesion force. In addition, forming a predefined formation is not important in the context of flocking. The control input (14) contains the desirable velocities defined in the inertial frame. Though a similar assumption is also introduced in [13], it is still different than the fully autonomous setting by Reynolds in a strict sense. The term is necessary since the group would stop after convergence without the term, which is far from the intuitive behaviors of flocking.

1.2

0.8

(pi)3[m]

j∈NCi

yi

xi

(ξiθi)2[rad]

for all i ∈ {1, · · · , n}, where the partial derivative of Uij (pi , pj ) with respect to pi is given in [40] and it depends only on the relative positions with respect to the neighbors. The first and second terms of the modified body velocity input (14) are the same as the input (13). If j ∈ NCi , then ∂U the term ∂piji works so that rigid body i moves away from rigid body j. Namely, the third term works as a separation rule. Therefore, the input (14) embodies all of the three rules by Reynolds. Theorem 4: Consider the n rigid bodies represented by (1) ˆ and the input (14). Assume that e¯ξi θi are positive definite for all i at the initial time, the interconnection graph GO is fixed, undirected and connected, wij = wji and P (0) ∈ / Ω. Then the velocity input (14) achieves attitude synchronization, while avoiding collisions. The positions finally converge to a configuration satisfying the following condition ∑ ∂Uij ∑ wij (qi − qj ) + = 0 ∀i. (15) ∂pi j∈NOi

0.4

Ci

Oi

zi

r 2 1.5

20

40

time[s]

60

0.5

0 1 2 3 4 5

−0.5

0.1 0 0

Fig. 6. 1)

20

40

time[s]

60

80

80

1

(ξiθi)3[rad]

where 0 < r < R. Under P (0) ∈ / Ω, Uij grows infinitely as rigid body i approaches rigid body j. By using this function, we present the body velocity input [ ] ˆ e−ξi θi vd b Vi = −ξˆ θ − e i i ωd  [ ] [ ˆ ] −ξi θi ∂Uij −ξî θi ∑ ∑ e e (qi − qj ) ∂pi (14) + Ki  wij , −ξˆj θj ξî θi ∨ sk(e e ) 0 j∈N j∈N

7

−1 0

20

40

time[s]

60

80

Time Responses of Position p and Rotation Vector ξθ (Simulation

that each rigid body is a rectangular solid whose breadth, width, height and circumradius are 1.2[m], 0.8[m], 0.6[m] and 1.5[m] respectively (Figure 4). Simulation 1 (Pose Synchronization): We consider the pose synchronization problem in Section III. Here we use five rigid bodies whose connection is represented by the graph in Figure 5. Note that this graph is strongly connected but not balanced. The input (4) with kpi = 0.5 and kei = 0.5 ∀i is applied to each rigid body under the following initial conditions p1 (0) = [2 0 6]T , ξ1 θ1 (0) = [0.21 0.50 0.77]T , p2 (0) = [4 − 2 − 4]T , ξ2 θ2 (0) = [0.60 0.04 0.83]T , p3 (0) = [6 2 4]T , ξ3 θ3 (0) = [−0.21 0.77 − 0.50]T , p4 (0) = [−2 − 4 0]T , ξ4 θ4 (0) = [−0.63 0.37 − 0.64]T , p5 (0) =


8

w43=w34 =300

3

w32=w23 =200 2

w12=w12 =100 1

Graph Topology in Simulation 2 Fig. 9.

0

1

20

−1 1 2 3 4 5

−2

0 −3

−1 −2 0

20

40

60

time[s]

80

−4 0

100

6

20

40

60

time[s]

80

0 0

20

40

60

time[s]

80

0.8 0.7

(ξiθi)2[rad]

0.6 0.5 0.4 0.3 0.2

1 2 3 4 5 20

40

60

time[s]

80

40

60

time[s]

80

100

8

−10 0

10 1 2 3 4 5

−1 −2

2

4

time[s]

6

8

0.4 0.2 0 1 2 3 4 5

−4 2

4

time[s]

6

8

−0.4 0

10

0.6

0.5

10

0.6

−0.2

100

−5 0

2

4

time[s]

6

8

10

1

0.4 0.2

0 1 2 3 4 5

−0.5

20

6

−3

1

0.1 0 0

(qi)3[m]

−0.2

−0.8 0

time[s]

0

−0.6

100 1 2 3 4 5

0

−1 0

20

40

60

time[s]

80

100

Fig. 8. Time Responses of Position p and Rotation Vector ξθ for Tij = 1[s]

[0 0 0]T , ξ5 θ5 (0) = [−0.77 0.50 − 0.21]T and biases di = [0 0 0]T ∀i. The initial state satisfies the assumption of Theorem 1, namely the rotation matrices are positive definite. Figure 6 shows time responses of position and rotation vectors. We see from Figure 6 the rotation vectors ξi θi and positions pi asymptotically converge to common values, that is, pose synchronization is achieved at around 50 [s]. Simulation 2 (Pose Synchronization in the Presence of Delays): We next check the validity of the present control input (7) in the presence of delays, where we use the same initial condition, gains and biases as Simulation 1 and the graph in Figures 7. Here, we run simulations with Tij = 1[s] for all connected i, j ∈ {1, · · · , 5}. Figure 8 shows time responses of position and rotation vectors for these delays. We see from the figures that periodicity is induced by the delays. However, the group at least synchronizes asymptotically and the validity of the present control input (7) is shown. Simulation 3 (Flocking Algorithm): In this simulation, we run simulations for control inputs (13) and (14). We select the radius of the collisional region as r = 3.0[m] since rigid bodies’ circumradius is 1.5[m]. The group consists of five rigid bodies with the kinematics described by (1) and a graph structure as depicted in Figure 9. This

(ξiθi)2[rad]

−4 0

4

1

−0.4

(ξiθi)3[rad]

(pi)3[m]

−2

(ξiθi)1[rad]

1 2 3 4 5

2

2

0.2

0

0 −5

5

0.4

2

10

5

100

0.6

4

15

1 2 3 4 5

10

(qi)2[m]

2

15

1 2 3 4 5

(ξiθi)1[rad]

3

25

1

(pi)2[m]

(pi)1[m]

4

i 1

5

Graph Topology in Simulation 3

2

1 2 3 4 5

(q ) [m]

6

w54=w45 =400 5

0 −0.2 1 2 3 4 5

−0.4 −0.6 −0.8 0

2

4

time[s]

6

8

10

(ξiθi)3[rad]

Fig. 7.

4

0.5

1 2 3 4 5

0

−0.5 0

2

4

time[s]

6

8

10

Fig. 10. Time Responses of Position p and Rotation Vector ξθ for Input (13) (Simulation 3)

graph is undirected and connected. The input (14) with vd = [1 0 0]T , ωd = [0 0 0]T , Kpi = 0.001I3 and kei = 0.001 ∀i is applied to each rigid body under the following conditions p1 (0) = [4 3 1]T , ξ1 θ1 (0) = [0.21 0.50 0.77]T , d1 = [0 0 0]T , p2 (0) = [2 3 − 5]T , ξ2 θ2 (0) = [0.60 0.04 0.83]T , d2 = [7 5 0]T , p3 (0) = [3 − 1 2]T , ξ3 θ3 (0) = [0.21 − 0.77 − 0.50]T , d3 = [7 − 5 0]T , p4 (0) = [−1 2 − 3]T , ξ4 θ4 (0) = [−0.32 0.48 − 0.32]T , d4 = [14 10 0]T , p5 (0) = [0 0 0]T , ξ5 θ5 (0) = [0.51 − 0.51 − 0.14]T , d5 = [14 − 10 0]T . We remark that the orientation matrices are positive definite at the initial time. We first run the simulation for the input (13). Figure 10 shows responses of position and rotation vector of each rigid body, and time responses of distances between rigid bodies are illustrated in Figure 11. In Figure 10 the rotation vectors ξi θi and positions pi asymptotically converge to a common value, that is, pose synchronization is achieved at around 6 [s]. We also see from the figure that every rigid body moves in the prescribed direction after sufficiently long time has passed. Thus, each rigid body finally moves with a desired velocity. However, as shown in Figure 11, rigid bodies 1 and 4 collide with rigid bodies 2 and 5, respectively.


15 1 1 1 1 2 2 2 3 3 4

10

5

3 0 0

1

2

3

& & & & & & & & & &

4

2 3 4 5 3 4 5 4 5 5

distances[m]

distances[m]

15

9

1 1 1 1 2 2 2 3 3 4

10

5

3

5

0 0

1

2

time[s]

3

4

& & & & & & & & & &

2 3 4 5 3 4 5 4 5 5 5

time[s]

Fig. 11. Time Responses of Dis- Fig. 12. Time Responses of Distances between Bodies for (13) tances between Bodies for (14)

Fig. 14.

Experimental Environment

Fig. 15.

Graph Topology in Experiment 1

3 z

5 4

1

2

1 x

Fig. 16. Fig. 13.

2

3

4

y

Trajectories of the Rigid Bodies with (14)

We next apply (14) to each body. Figure 12 shows time responses of distances between rigid bodies and Figure 13 shows the trajectories of the rigid bodies. In Figure 13, the encircled number is associated with the corresponding one in Figure 9. We see from these figures that the rigid bodies smoothly adjust their orientation without collisions. From the figures we can confirm that attitude synchronization and collision avoidance are achieved by the body velocity input (14). In the case of this simulation, the positions also converge to the desired one. VII. E XPERIMENTS In this section we present experimental results on a planar (2D) test bed. The objective of this experiment is to demonstrate that the results of this paper are valid even in the presence of the many disturbances sources in the hardware experiments, such as the vehicle dynamics, imperfect actuation, friction of wheels and imperfect tracking of lowlevel controllers.

Graph Topology in Experiment 2

wireless communication device Wiport (LANTRONIX). Since the robot has underactuated characteristics, a local controller due to Astolfi [50] is embedded to the microcomputer so that it tracks to any desirable position and orientation and we use the present velocity control law as a high-level controller generating desirable positions and orientations. Here, we let these desirable values be Ts Vib with Ts = 1[s]. Although we have not considered pose synchronization of rigid bodies with nonholonomic constraints theoretically yet and it is one of future works of this paper, we believe that the techniques in [51] will be helpful for this investigation. Figure 14 illustrates the experimental environment including the robots, camera and PC. Further information on the experimental setup will be shown in [52]. In this section, we carry out two experiments with different communication graphs. For both experiments, let the gains in (4) be kpi = 1, kei = 1, the weights of graphs be wij = [ ]T 1 ∀i, j and the desired velocity be given by vd = 0.06 0 m/s and ωd = 0 rad/s. The virtual positions are defined by d1 = [−0.240 − 0.240]T m, d2 = [0.240 − 0.240]T m, d3 = [0.240 0.240]T m, d4 = [−0.240 0.240]T m in order to avoid collisions at the final configuration.

A. Experimental Environment In this experiment, we use four wheeled mobile robots. A MTV-7310 camera mounted above the robots has a resolution of 470 × 570. The video signals are available in real time via a frame grabber board PicPort-Stereo-HrD and image processing software HALCON. The sampling period of the controller and the frame rate provided by the camera are 0.33[ms] and 30 [fps], respectively. The position and orientation of the robots are calculated by using the image processing. Based on these information, the PC computes our velocity control input Vib and send it to each robot via the embedded

B. Experiment 1 (Pose Synchronization) We first assume that the mobile robots are communicated by the graph in Figure 15 whose algebraic connectivity is 2. The experiment is carried out with the initial condition p1 (0) = [0.196 1.202]T m, θ1 (0) = −0.505 rad, p2 (0) = [0.462 0.826]T m, θ2 (0) = 0.594 rad, p3 (0) = [0.451 0.495]T m, θ3 (0) = 0.744 rad, p4 (0) = [0.255 0.188]T m, θ4 (0) = 0.776 rad. Note that the initial orientations guarantee that all the relative orientation matrices are positive definite.


1.5

1 2 3 4

1

1.5

1 2 3 4

1

0.5

1

0 -0.5

0.5

0.5

0 -0.5

-1

0.5

1 x [m]

1.5

2

-1.5

-1

0

5

time [s]

10

15

Fig. 17. Trajectories of Mobile Fig. 18. Time Responses of OrienRobots (Experiment 1) tations (Experiment 1) 2

1 2 3 4

1.5

1 x [m]

1.5

2

1

2

0

5

time [s]

10

15

1.4

1 2 3 4

1.5

1 2 3 4

1.2 1

0.8 x [m]

1

0.5

-1.5

Fig. 21. Trajectories of Mobile Fig. 22. Time Responses of OrienRobots (Experiment 2) tations (Experiment 2)

1 2 3 4

1.2

y [m]

x [m]

1.4

0 0

0.6

y [m]

0 0

1 2 3 4

1

y [m]

y [m]

angle [rad]

0.5

1.5

1 2 3 4 angle [rad]

1.5

10

1

0.8 0.6

0.4

0.5

0.4

0.5

0.2 0.2

0 0

5

time [s]

10

15

0 0

5

time [s]

10

15

Fig. 19. Time Responses of x- Fig. 20. Time Responses of ycoordinates (Experiment 1) coordinates (Experiment 1)

The experimental results are shown in Figures 17 – 20. Figure 17 illustrates the trajectories of the robots on the 2D plane, and Figures 18 – 20 the time responses of orientations, the x-coordinates and y-coordinates of the positions, respectively. In Figure 17, the circles denote the initial positions of the vehicles. We see from Figures 17 and 18 that orientations asymptotically converge to a common value, positions converge to the desired relative configuration, and all the bodies eventually move in the same direction specified by vd and ωd . This means that the present velocity control law achieves pose synchronization. Moreover, Figures 19 and 20 show that x- and y-coordinates almost converge to the desired relative values at around 5[s], and 8[s], respectively. These results will be utilized for the comparison with respect to the graph structures in the next subsection. C. Experiment 2 (Convergence Speed) In this subsection, we assume that the communications between the robots are represented by the graph in Figure 16 whose algebraic connectivity is 0.5858. The initial condition of this experiment is p1 (0) = [0.268 1.163]T m, θ1 (0) = −0.602 rad, p2 (0) = [0.501 0.827]T m, θ2 (0) = 0.605 rad, p3 (0) = [0.471 0.505]T m, θ3 (0) = 0.767 rad, p4 (0) = [0.253 0.114]T m, θ4 (0) = 0.890 rad. Note that the initial orientations also guarantee that all the relative orientation matrices are positive definite. The experimental results are shown in Figures 21 – 24, which correspond to Figures 17 – 20, respectively. We see from these figures that the pose synchronizes, the orientations do not converge to a common value completely even at the end of the experimental, and x- and y-coordinates converge to a desirable relative configuration at around 9[s] and 14[s], respectively. Namely, the graph structure in Figures 16 achieves faster convergence than that of Figure 15. These results suggest that the bigger λmin2 (Lγsym ) is, the faster convergence is attained as stated in Section III-C. Since the initial values cannot be exactly the same as those in the previous subsection

0 0

5

time [s]

10

15

0 0

5

time [s]

10

15

Fig. 23. Time Responses of x- Fig. 24. Time Responses of ycoordinates (Experiment 2) coordinates (Experiment 2)

due to a characteristic of the robots, this comparison might not be fair. However, the difference is not so large and at least the above tendency is true. Indeed, similar results are obtained though we perform experiments under several different initial conditions. It should be finally noted that the sensing system of the experimental system is centralized and hence the input is not produced in a distributed fashion in this sense. Consequently, the effects of relative sensing devices on synchronization are not demonstrated through the experiments in this paper and further investigations on the issue from both theoretical and experimental viewpoint should be carried out in the future. VIII. C ONCLUSIONS In this paper, we have investigated pose synchronization on SE(3) based on a passivity-like property of the kinematics of rigid bodies. We first developed a passivity-based control law attaining pose synchronization. It also has been shown that the pose synchronizes exponentially fast in a local region with an exponential convergence rate given by algebraic connectivity of interconnection graphs. We also have prove pose synchronization in the presence of communication delays and topology switches. We moreover have given further extensions of the present law, where desirable velocities and collision avoidance are taken into account. The simulation and experimental results have demonstrated the validity of our results. A further direction of this research is to extend the results of this paper to pose synchronization in SE(3) for multiple rigid bodies with dynamics and introduction of visual sensors to gain relative information with respect to neighbors [44]. This work was partially supported by Grant-in-Aid for Scientific Research (C) No. 19560437 and by the US National Science Foundation under grant ECCS 07-25433 ACKNOWLEDGMENT The author would like to thank Mr. T. Ibuki for invaluable help in carrying out the experiments.


A PPENDIX A P ROOF OF T HEOREM 1 Throughout the appendices, we omit the detailed development of equations due to the page constraints. Referring to [33] would be a help in understanding them. A. Proof of Lemma 2 The following Lemma is necessary in order to prove Theorem 1. Lemma 2: Consider n rigid bodies represented by (1) and suppose that the velocity input (4) is applied to each body. ˆ ˆ ˆ ˆ If there exists eξα θα such that e¯ξi θi = e−ξα θα eξi θi ∀i are ˆ positive definite at the initial time, then e¯ξi θi remain to be positive definite for all subsequent time. Proof: In this proof, we use the fact that the positive ˆ definiteness of the rotation matrix eξi θi is equivalent to ˆ ϕ(eξi θi ) < 1. Let ι(t) ∈ {1, · · · , n}, t ≥ 0 denote the rigid body with the maximal energy function at time t, i.e. ˆ

11

where γi are vectors satisfying γ T Lw = 0, γ T = [γ1 , · · · , γn ], γi > 0 ∀i

(17)

Without loss of generality, we assume that the vector γ is normalized, i.e. satisfies ∥γ∥ = 1 throughout this paper. Differentiating (16) yields ][ ] [ ]T [ 1 n ˆ ∑ 0 qi eξi θi 0 kpi I ˙ UO = γi Vib 1 ξî θi ∨ 0 I 0 I sk(¯ e ) k ei i=1 n { ∑ ∑ = γi wij qiT (qj − qi ) i=1 j∈NOi

(( )−1 )∨ } ˆ ˆ ˆ +(sk(¯ eξi θi )∨ )T sk e¯ξi θi e¯ξj θj . (18)

By completing the square, the term qiT (qj − qi ) is rewritten as 1 1 1 qiT (qj − qi ) = − ∥qi ∥2 + ∥qj ∥2 − ∥(qi − qj )∥2 . 2 2 2 We next obtain

(19)

(( )−1 )∨ ˆ ˆ ˆ ˆ ˆ (sk(¯ eξi θi )∨ )T sk e¯ξi θi e¯ξj θj = −ϕ(¯ eξi θi ) + ϕ(¯ eξj θj ) ˆ ( )−1 ) Then, the derivative of this potential function ϕ(¯ eξι θι ) along 1 (( ξî θi ( ξî θi )−1 ) ξî θi ξˆj θj (20) tr e ¯ + e ¯ (I − e ¯ e ¯ ) − the trajectory of the kinematics model (1) with (4) is given by 4 ( )T ˆ ˙ eξˆι θι )= sk(¯ from the fact that aT b = − 12 tr(ˆ aˆb) holds for any 3 diϕ(¯ eξι θι )∨ ωι 3 3 mensional vector a ∈ R , b ∈ R [33, Theorem 1]. Since ( )T (( )−1 )∨ ∑ ˆ ˆ ˆ = kι ωιj sk(¯ eξι θι )∨ sk e¯−ξι θι e¯ξj θj . λmin (B)tr(A) ≤ tr(AB) holds true for any positive semidefinite symmetric matrices A ∈ Rn×n , B ∈ Rn×n [53], we j∈Nι have Calculations similar to the subsequent (20) and (21) yield (( ( )−1 )( ( )−1 )) ( ∑ ξî θi ξî θi ξˆj θj ξî θi ˆ ˆ ˆ −tr e ¯ + e ¯ I − e ¯ e ¯ ξ θ ξ θ ξ θ ι ι ι ι j j ˙ e )≤ ϕ(¯ kι ωιj − ϕ(¯ e ) + ϕ(¯ e ) ( ( )−1 ) ( ( )−1 ) j∈Nι ξî θi ξî θi ξˆj θj ξî θi ≤ −λ e ¯ + e ¯ tr I − e ¯ e ¯ (, 21) ( ( ) ) (( ) )) min −1 −1 ˆ 1 ˆ ˆ ˆ ϕ e¯ξι θι e¯ξj θj . − λmin e¯ξι θι + e¯ξι θι 2 where( λmin (B) ( denotes )−1 )the minimal eigenvalue of B. Notice î θi î θi ˆ ξˆι θι ξˆj θj ξ ξ It is clear from the −ϕ(¯ e ) ) + ϕ(¯ e ))) ≤ λmin e¯ + e¯ > 0 since e¯ξi θi > 0 ∀i holds for ( definition ( of ι)that −1 ) (( −1 ˆ ˆ ˆ ˆ 0 ∀j. Since λmin e¯ξι θι + e¯ξι θι ϕ e¯ξι θι e¯ξj θj ≥ all time from Lemma 2. Thus, the deviation of the potential function (16) satisfies the inequality ξˆι θι ˙ 0, the inequality ϕ(¯ e ) ≤ 0 holds true. We thus obtain n ( ∑ ∑ ˆ ˆ ˆ 1 ϕ(¯ eξi (t)θi (t) ) ≤ ϕ(¯ eξι (t)θι (t) ) ≤ ϕ(¯ eξι (0)θι (0) ) < 1 U˙ O ≤ γi wij − Ui + Uj − ∥(qi − qj )∥2 2 i=1 j∈NOi for all i ∈ {1, · · · , n} and t ≥ 0 from the assumption that ( ( )−1 ) (( )−1 )) 1 ˆ ˆ ˆ ˆ the rotation matrices are positive definite at the initial time. − λmin e¯ξi θi + e¯ξi θi ϕ e¯ξi θi e¯ξj θj , (22) 2 This inequality means all the orientation matrices are positive ˆ 1 definite for any time t ≥ 0. This completes the proof. where := eξi θi ). Now, the term 2 ∥qi ∥ + ϕ(¯ ∑n ∑Ui i=1 j∈NOi γi wij (−Ui + Uj ) is equivalent to B. Proof of Theorem 1 n ∑ ∑ [ ]T In order to prove the theorem, we use the following Lemma γi wij (−Ui + Uj )=−γ T Lw U1 · · · Un = 0. (23) on properties of the graph Laplacian without proof. i=1 j∈NOi Lemma 3: 1) If the graph is strongly connected and the weights are positive, there exists a vector γ satisfying γ T Lw = This yields the following inequality, and hence the deviation 0 whose elements are positive [9]. 2) If the graph is strongly of the potential function is non-positive definite. n connected, rank(Lw ) = n − 1 [8]. ( 1∑ ∑ ˙ UO ≤ − γi wij ∥qi − qj ∥2 Define a potential function by 2 i=1 ( ) j∈NOi n ∑ 1 1 ˆ )) )−1 )−1 ) (( ( ( 2 ξi θi UO := γi ∥qi ∥ + ϕ(¯ e ) , (16) ˆ ˆ ˆ ˆ ≤ 0 (24) e¯ξj θj ϕ e¯ξi θi +λmin e¯ξi θi + e¯ξi θi 2kpi kei ι(t) := arg

max

i∈{1,··· ,n}

i=1

ϕ(¯ eξi θi (t) ).


12

We finally prove the convergence of the output by using the LaSalle’s Invariance Principle [54]. Before proving it, we define the set ˆ E := {yi ∈ SE(3), ∀i | e¯ξi θi > 0 U˙ O ≡ 0}.

(25)

ˆ From e¯ξi θi > 0 and (24), if U˙ O = 0,

(( )−1 ) ˆ ˆ ∥qi − qj ∥ = 0, ϕ e¯ξi θi e¯ξj θj = 0, (j, i) ∈ E 2

= {gi ∈ SE(3), ∀i | e¯

>0

ψ(yi−1 yj )

(26)

= 0 ∀i, j}. (27)

In addition, the inputs (4) of all the rigid-bodies are equal in the case of yi = yj ∀i, j. This implies that the set E is an invariant set. Consequently, we have (3) and the pose synchronization is achieved.

A. Proof of Lemmas The following Lemmas are necessary in order to prove Theorem 2. Lemma 4: Consider n rigid bodies represented by (1) and suppose that the velocity input (4) is applied to each body. If ˆ ˆ the relative orientations e−ξi θi eξj θj ∀i, j are positive definite ˆ ˆ at the initial time, then e−ξi θi eξj θj ∀i, j remain to be positive definite for all subsequent time. Proof: Here, we consider only the orientation part of the group behavior. Let (i′ (t), j ′ (t)) be a pair satisfying ˆ

ˆ

ˆ

ϕ(e−ξi θi (t) eξj θj (t) ) ≤ ϕ(e−ξi′ (t) θi′ (t) eξj′ (t) θj ′ (t) ) ∀i, j (28) ˆ

at time t ≥ 0. The derivative of ϕ(e−ξi′ θi′ e−ξj′ θj′ ) along the trajectory of (1) is given by ˆ

˙ −ξî′ θi′ e−ξˆj′ θj ′ ) ϕ(e )T ( ˆ ˆ = sk(e−ξi′ θi′ eξj′ θj ′ )∨ (−ωi′ + ωj ′ ) )T ( ∑ ˆ ˆ ˆ ˆ = −kei′ wi′ i sk(e−ξi′ θi′ eξj ′ θj ′ )∨ sk(e−ξi′ θi′ eξi θi )∨ i∈Ni′

+kej ′

∑

j∈Nj ′

)T ( ˆ ˆ ˆ ˆ wj ′ j sk(e−ξi′ θi′ eξj ′ θj ′ )∨ sk(e−ξj′ θj′ eξj θj )∨

( ) kei′ ∑ ˆ ˆ ˆ ˆ wi′ i tr sk(e−ξi′ θi′ eξj ′ θj ′ )sk(e−ξi′ θi′ eξi θi ) 2 i∈Ni′ ( ) ′ kej ∑ ˆ ˆ ˆ ˆ wj ′ j tr sk(e−ξi′ θi′ eξj′ θj′ )sk(e−ξj′ θj ′ eξj θj ) (29) − 2

=

j∈Nj ′

1 1 ˆ ˆ ˆ ˆ ˆ ˆ tr(e−ξi′ θi′ eξj θj ) − tr(e−ξi′ θi′ eξj′ θj′ e−ξj θj eξj′ θj′ ) 2 2 (29) is rewitten as ˙ −ξî′ θi′ eξˆj ′ θj ′ ) ϕ(e ( kei′ ∑ ˆ ˆ = wi′ i tr − e−ξj′ θj′ eξi θi 4 i∈Ni′

ˆ

ˆ

ˆ

ˆ

+e−ξi′ θi′ eξj′ θj ′ e−ξi′ θi′ eξi θi +

)

( kej ′ ∑ ˆ ˆ wj ′ j tr − e−ξi′ θi′ eξj θj 4 j∈Nj ′

ˆ

ˆ

ˆ

ˆ

+e−ξi′ θi′ eξj′ θj ′ e−ξj θj eξj′ θj′

) (30)

Let us now consider the term ) ( ˆ ˆ ˆ ˆ ˆ ˆ tr −e−ξj′ θj′ eξi θi + e−ξi′ θi′ eξj′ θj′ e−ξi′ θi′ eξi θi . Then, we have ( ) ˆ ˆ ˆ ˆ ˆ ˆ tr − e−ξj ′ θj ′ eξi θi + e−ξi′ θi′ eξj ′ θj ′ e−ξi′ θi′ eξi θi

A PPENDIX B P ROOF OF T HEOREM 2

ˆ

1 1 ˆ ˆ ˆ ˆ ˆ ˆ = − tr(e−ξj′ θj′ eξi θi ) + tr(e−ξi′ θi′ eξj′ θj′ e−ξi′ θi′ eξi θi ) 2 2 ( ) ˆ ˆ ˆ ˆ tr sk(e−ξi′ θi′ eξj ′ θj ′ ))sk(e−ξj′ θj′ eξj θj ) =

holds. Because of the strong connectivity of the graph, the set E is replaced by { ˆ E = gi ∈ SE(3), ∀i e¯ξi θi > 0, ∥qi − qj ∥2 = 0, (( ) )−1 } ˆ ˆ ϕ e¯ξi θi e¯ξj θj = 0 ∀i, j ξî θi

Now, since ( ) ˆ ˆ ˆ ˆ tr sk(e−ξi′ θi′ eξj ′ θj ′ )sk(e−ξi′ θi′ eξi θi )

ˆ

ˆ

ˆ

ˆ

ˆ

ˆ

ˆ

ˆ

= −ϕ(e−ξi′ θi′ eξj ′ θj ′ ) + ϕ(e−ξj′ θj ′ eξi θi ) ( ) ˆ ˆ ˆ ˆ −tr e−ξi′ θi′ eξj′ θj′ (I − e−ξi′ θi′ eξi θi ) . ≤ −ϕ(e−ξi′ θi′ eξj ′ θj ′ ) + ϕ(e−ξj′ θj ′ eξi θi ) ( ) 1 ˆ ˆ ˆ ˆ ˆ ˆ − λmin (e−ξi′ θi′ eξj′ θj′ + e−ξj ′ θj ′ eξi′ θi′ )ϕ e−ξi′ θi′ eξi θi . 2 ˆ

ˆ

From (28), −ϕ(e−ξi′ θi′ eξj′ θj ′ ) + ϕ(e−ξj ′ θj ′ eξi θi ) ≤ 0 and, ˆ ˆ from the assumption of e−ξi θi eξj θj > 0 ∀i, j, we get ˆ ˆ ˆ ˆ ˆ ˆ λmin (e−ξi′ θi′ eξj′ θj′ + e−ξj ′ θj′ eξi′ θi′ )ϕ(e−ξi′ θi′ eξi θi ) ≥ 0, which yields ) ( ˆ ˆ ˆ ˆ ˆ ˆ tr −e−ξj ′ θj′ eξi θi + e−ξi′ θi′ eξj′ θj′ e−ξi′ θi′ eξi θi ≤ 0 ∀i. (31) ˆ

ˆ

Namely, the first term of (30) is not positive. Similarly, the non-positiveness of the second term is proved in the same ˙ −ξî′ θi′ eξˆj′ θj′ ) ≤ 0 way as above. We thus can conclude ϕ(e and hence ˆ

ˆ

ˆ

ˆ

ˆ

ˆ

ϕ(e−ξi θi (t) eξj θj (t) )≤ϕ(e−ξi′ θi′ (t) eξj′ θj′ (t) ) ≤ϕ(e−ξi′ θi′ (0) eξj′ θj ′ (0) ) ∀i, j, t ≥ 0. This completes the proof. Lemma 5: Define the function ) n ∑ n ( ∑ 1 γi γl γi γl 2 −ξî θi ξˆl θl ˜ Uexp:= ∥qi − ql ∥ +2 ϕ(e e ) , 2 kpi kpl kei kel i=1 l=1

˜exp satisfies the where γi and γl are given by (17). Then, U inequality ( ) γi γl γi γl ˜exp min , Uexp ≤U i,l kpi kpl kei kel


13

( ) γi γl γi γl ≤max , Uexp . (32) i,l kpi kpl kei kel

=−

n 2 ∑ γl T Q ((M Lγsym M ) ⊗ I3 ) Q. n2 kpl

(37)

l=1

˜exp , we immediProof: By the definition of Uexp and U From the fact that aT b = − 12 tr(ˆ aˆb) holds for any 3 dimenately obtain sional vectors a ∈ R3 and b ∈ R3 , we obtain )n n( ( ) n ∑ n ∑ )T γi γl ∑∑ 1 γi γl ∑ 2 −ξî θi ξˆl θl γi γl ( ˆ ˆ ˆ ˆ min , ∥qi − ql ∥ +2ϕ(e e ) −4 wij sk(e−ξi θi eξl θl )∨ sk(e−ξi θi eξj θj )∨ i,l kpi kpl kei kel 2 k i=1 l=1 l i=1 l=1 j∈NOi ˜exp ≤U n n ( ) ∑ ( )∑ ( ) γl ∑ ∑ n ∑ n ˆ ˆ ˆ ˆ ˆ ˆ γi γl γi γl 1 ˆ ˆ = γi wij tr e−ξi θi eξl θl e−ξi θi eξj θj −e−ξl θl eξj θj . 2 −ξi θi ξl θl ≤ max , ∥qi − ql ∥ +2ϕ(e e ) kl i=1 j∈NOi l=1 i,l kpi kpl kei kel i=1 2 l=1

and

Making a calculation similar to (36), we obtain n ∑ n ( ∑

)

1 ˆ ˆ ∥qi − ql ∥2 + 2ϕ(e−ξi θi eξl θl ) 2 i=1 l=1 ( ) ˆ ˆ = QT (M ⊗ I3 )Q + tr (eξθ )T (M ⊗ I3 )eξθ = Uexp .

Thus, (32) is satisfied. Lemma 6: There exists an ϵ > 0 satisfying ˜˙ exp ≤ − ϵ λmin2 (Lγsym )Uexp . (33) U n ˜exp along trajectories of (1) is Proof: The derivative of U given by [ ]T[ ] γi γl n ∑ n ∑ (pi − pl ) q˙i − q˙l k k ˙ pi pl ˜exp = U ˆ ˆ −ωi + ωl 2 kγeii γklel sk(e−ξi θi eξl θl )∨ i=1 l=1 n ∑ n ∑ ∑ = γi wij × i=1 l=1 j∈NOi ]T [ 2 kγpll (qi − ql ) × ˆ ˆ 4 kγell sk(e−ξi θi eξl θl )∨

[

qj − qi −ξˆj θj ξî θi ∨

sk(e

e

)

] (34)

∑n ∑n ∑ The term 2 i=1 l=1 j∈NOiγi wij kγpll (qi − ql )T (qj − qi ) satisfies n ∑ n ∑ ∑ γl 2 γi wij (qi − ql )T (qj − qi ) k pl i=1 l=1 j∈NOi n n ∑ ( ) γl ∑ ∑ = γi wij −∥qi ∥2 +∥qj ∥2 −∥qi −qj ∥2 kpl i=1 j∈NOi l=1 n n ∑ γl T ∑ ∑ γi wij (qj − qi ). (35) −2 ql kpl i=1 j∈NOi

l=1

Using∑the weighted graph Laplacian, we can show that the n ∑ term i=1 j∈NOi γi wij (−∥qi ∥2 +∥qj ∥2 ) satisfies n ∑ ∑

[ ]T γi wij (−∥qi ∥ +∥qj ∥ ) = −γ Lw ∥q1 ∥2 · · · ∥qn ∥2 2

2

T

i=1 j∈NOi

= 0.

(36)

∑n ∑ Similarly, i=1 j∈NOi γi wij (qj − qi ) = 0. Thus equation (35) can be rewritten as −

n ∑

n ∑

∑ γl γi wij ∥qi −qj ∥2 kpl i=1 j∈NOi l=1 n ∑ 2γl T Q (Lγ ⊗ I3 )Q =− kpl l=1

n ∑ ∑

ˆ

γi wij eξj θj =

i=1 j∈NOi

n ∑ ∑

ˆ

γi wij eξi θi .

(38)

i=1 j∈NOi

It follows from (38) that n n ∑ γl ∑ ∑

kl

l=1

ˆ

i=1 j∈NOi n ∑

=

l=1

and that n n ∑ γl ∑ ∑

l=1

kl i=1

kl

=−

( ) ˆ ˆ ˆ ˆ γi wij tr e−ξl θl eξj θj − e−ξl θl eξi θi = 0.(40)

( ) ˆ ˆ ˆ ˆ ˆ ˆ γi wij tr e−ξi θi eξl θl e−ξi θi eξj θj −e−ξl θl eξi θi

i=1 j∈NOi

n n ∑ γl ∑ ∑ l=1

kl

(39)

j∈NOi

j∈NOi

Hence we have n n ∑ γl ∑ ∑ l=1

n γl ∑ ∑ ˆ ˆ γi wij e−ξl θl eξi θi kl i=1

( ) ˆ ˆ ˆ ˆ γi wij tr e−ξl θl eξi θi (e−ξi θi eξj θj − I)

kl i=1 j∈NOi l=1 n n ∑ γl ∑ ∑

=

ˆ

γi wij e−ξl θl eξj θj

( ˆ ˆ ˆ ˆ γi wij tr (e−ξi θi eξl θl + e−ξl θl eξi θi )× ) ˆ ˆ ×(I −e−ξi θi eξj θj ) .

i=1 j∈NOi

ˆ

(41)

ˆ

Since the rotation matrices e−ξi θi eξj θj ∀i, j are positive definite for all time from Lemma 4, they satisfy the following inequality [53] ( ) ˆ ˆ ˆ ˆ ˆ ˆ −tr (e−ξi θi eξl θl +e−ξl θl eξi θi )(I −e−ξi θi eξj θj ) ( ) ˆ ˆ ˆ ˆ ˆ ˆ ≤ −λmin (e−ξi θi eξl θl +e−ξl θl eξi θi )tr I −e−ξi θi eξj θj . (42) Substituting the inequality (42) into equation (41) yields n ∑ n ∑ )T ∑ γi γl ( ˆ ˆ ˆ ˆ wij sk(e−ξi θi eξl θl )∨ sk(e−ξi θi eξj θj )∨ −4 k el i=1 l=1 j∈NOi

n n ∑ γl ∑ ∑ ≤− γi wij × kel i=1 l=1

j∈NOi −ξî θi ξˆl θl

×λmin (e

e

ˆ

) ( ˆ ˆ ˆ ˆ + e−ξl θl eξi θi )tr I − e−ξi θi eξj θj ˆ

ˆ

ˆ

≤ − min λmin (e−ξi θi (t) eξl θl (t) + e−ξl θl (t) eξi θi (t) )× i,l,t n ) ∑ γl ( ξθ ˆ ˆ × tr (e )T (Lγ ⊗ I3 )eξθ kel l=1


14

1 ˆ ˆ ˆ ˆ min λmin (e−ξi θi (t) eξl θl (t) + e−ξl θl (t) eξi θi (t) )× n2 i,l,t n ) ∑ γl ( ξθ ˆ ˆ × tr (e )T (M Lγsym M ⊗ I3 )eξθ . (43) kel

= −

l=1

Let us now define (

) n ∑ γl ϵ := min min 2 , ϵ˜ > 0. i,l,t kpl l=1

−ξî θi (t) ξˆl θl (t)

ϵ˜ := λmin (e

e

ˆ

ˆ

+ e−ξl θl (t) eξi θi (t) )

n ∑ γl kel l=1

From this definition, (34), (37) and (38) can be rewritten as ( ( ) ˜˙ exp ≤ − ϵ QT (M Lγsym M ) ⊗ I3 Q U n2 ( )) ˆ ˆ (44) +tr (eξθ )T (M Lγsym M ⊗ I3 )eξθ Due to a property of the Kronecker product, 1T (M ⊗ ˆ I3 )eξθ x = 0 ∀x and the Courant-Fischer theorem [53], the following inequality is satisfied. ˆ

ˆ

λmin2 (Lγsym )xT (eξθ )T (M ⊗ I3 )(M ⊗ I3 )eξθ x ˆ ˆ ≤ xT (eξθ )T (M ⊗ I3 )(Lγsym ⊗ I3 )(M ⊗ I3 )eξθ x This implies that ( ) ˆ ˆ −tr (eξθ )T (M ⊗ I3 )(Lγsym ⊗ I3 )(M ⊗ I3 )eξθ ( ) ˆ ˆ ≤ −λmin2 (Lγsym )tr (eξθ )T (M ⊗ I3 )(M ⊗ I3 )eξθ ( ) ˆ ˆ = −nλmin2 (Lγsym )tr (eξθ )T (M ⊗ I3 )eξθ . (45)

The proof of (9) is thus completed taking ( ) maxi,l kγpii kγlpl , kγeii γklel 1 ϵ ( ( ) , b := ). a := γi γl γi γl n max mini,l kγpii kγlpl , kγeii γklel i,l kpi kpl , kei kel (48) The above parameters a and b depend on the graph since γ is a normalized left eigenvector of the graph Laplacian Lw . However, under the assumption (10), the maximum of a and minimum b with respect to the graphs (denoted by a ˜ and and ˜b, respectively) exist and finite. By using a ˜ and ˜b as a and b, (9) holds true for a and b independent of the graph structure. This completes the proof. A PPENDIX C P ROOF OF T HEOREM 3 Proof: If the graph is strongly connected, rank(Lw ) = n − 1 [8]. It follows that if s(t) ∈ Gdc , then the algebraic connectivity satisfies λmin2 (Lγsym (s(t))) = 0 and λmin2 (Lγsym (s(t))) > 0 otherwise. By using the characteristic function, the inequality (9) can be rewritten on the interval [t, ti ) as Uexp (t) ≤ a∗ Uexp (ti )e−λ

∗

(1−X (s(t)))(t−ti )

,

(49)

where a∗ := mins(t)∈G a, λ∗ := mins(t)∈Gc λmin2 (Lγsym )b, and a, b are defined in the proof of Theorem 2 (Appendix B). The inequality (49) implies that ∗

Uexp (t) ≤ a∗ Uexp (ti )e−λ

(t−ti −T (ti ,t))

(50)

Similarly, −QT ((M Lγsym M ) ⊗ I3 ) Q ≤ −nλmin2 (Lγsym )QT (M ⊗ I3 ) Q.

(46)

The inequality (33) now follows from the inequalities (44), (45) and (46).

true on the interval [t, ti ), where T (ti , t) := ∫holds t X (s(r))dr = X (s(t))(t − ti ). Similarly, it follows from ti (9) that the function Uexp satisfies Uexp (ti ) ≤ a∗ Uexp (ti−1 )e−λ

∗

(ti −ti−1 −T (ti−1 ,ti ))

(51)

on the interval [ti , ti−1 ). Substituting (51) to (50) yields the inequality

B. Proof of Theorem 2

Uexp (t) ≤ (a∗ )2 Uexp (ti−1 )e−λ

From Lemmas 5 and 6, we get λmin2 (Lγsym ) ˜˙ exp ≤ − ϵ ˜exp . ( )U U γi γl n max , γi γl i,l

kpi kpl

i,l

kpi kpl

Uexp (t) ≤ Uexp (0)e−λ

(47)

kei kel

where exp(x) = ex . The inequalities (32) and (47) together imply the inequality ( ) maxi,l kγpii γklpl , kγeii γklel ) Uexp (0)× ( Uexp ≤ mini,l kγpii γklpl , kγeii γklel ( ϵ ) λmin2 (Lγsym ) ( )t . ×exp − γi γl n max , γi γl i,l

kpi kpl

kei kel

(t−ti−1 −T (ti−1 ,t))

.

By iterating the above calculation, we obtain

kei kel

It follows from the comparison principle [54] that   ϵ λ (L ) min2 γsym ˜exp ≤ U ˜exp (0)exp − ( ) t , U γi γl γi γl n max ,

∗

∗

(t−T (0,t))+Ns (0,t) ln(a∗ )

,

where Ns (τ, t) denotes the number of graph switches over the interval (τ, t). From the assumptions of the theorem, T (τ, t) ≤ α(t − τ ) + T0 and Ns (0, t) ≤ τtD are satisfied and hence ( ∗ ) ∗ ln(a∗ ) −λ (1−α)+ τ t−λ T0 D Uexp (t) ≤ Uexp (0)e . ∗

) This inequality implies that if −λ∗ (1 − α) + ln(a < 0 τD is satisfied, then we have limt→∞ Uexp (t) = 0, which is a necessary and sufficient condition for pose synchronization. Thus a sufficient conditions for limt→∞ Uexp (t) = 0 is given a∗ , which means the existence of a lower bound by τD > λ∗ln(1−α) of τD to achieve pose synchronization. This completes the proof.


A PPENDIX D P ROOF OF T HEOREM 4 Proof: The function UC =

n 1∑ ∑ wij ∥qi − qj ∥2 2 i=1 j∈NOi n ∑ n n ∑ ∑ 1 ˆ + Uij (pi , pj ) + ϕ(¯ eξi θi ) k ei i=1 j=1 i=1

is used as a potential function for the whole system. Taking its derivative along the trajectories of the kinematics model (1) in the region D, we have ∑ ∂Uij )T Kpi × ∂pi i=1 j∈NOi j∈NCi ( ∑ ∑ ∂Uij ) × wij (qi − qj ) + ∂pi j∈NOi j∈NCi ( n ∑ ( )−1 ) ( ) ∑ 1 ˆ ˆ ξî θi ξî θi − wij λmin e¯ + e¯ ϕ e−ξi θi eξj θj 2 i=1

U˙ C ≤ −

n ( ∑ ∑

wij (qi − qj ) +

j∈NOi

whose right-hand side is nonpositive. Thus, we can see that the potential function UC is non-increasing in the region D. Notice if no collision occurs, the potential function is finite. Due to the assumption of P (0) ∈ / Ω and lim∥pi −pj ∥→r+ UC = +∞, ∃i, j, i ̸= j, we conclude that P (t) will never belong to Ω, that is, collisions do not happen for all time t ≥ 0. Next we prove attitude synchronization using LaSalle’s Invariance ˆ Principle [54]. Let us now define the set EA := {¯ eξi θi ∈ ˆ SO(3), ∀i |¯ eξi θi > 0 U˙ C = 0}. Then, the set EA is characterˆ ˆ ized by all trajectories satisfying ϕ(e−ξi θi eξj θj ) = 0 (j, i) ∈ E. Therefore LaSalle’s Invariance Principle [54] and connectivity of the information graph proves attitude synchronization. R EFERENCES [1] F. Bullo, J. Cortes and S. Martinez, “Distributed Control of Robotic Networks,” Princeton Series in Applied Mathematics, 2009. [2] P. Ogren, E. Fiorelli and N. E. Leonard, “Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in A Distributed Environment,” IEEE Transactions on Automatic Control, Vol. 49, No. 8, pp. 1292–1302, 2004. [3] N. E. Leonard, D. A. Paley, F. Lekien, R. Sepulchre, D. M. Fratantoni and R. E. Davis “Collective Motion, Sensor Networks, and Ocean Sampling,” Proceedings of the IEEE — Vol. 95, No. 1, pp. 48–74, 2007 [4] R. M. Murray, “Recent Research in Cooperative Control of Multivehicle Systems,” Journal of Dynamic Systems Measurement and ControlTrans. of the Asme, Vol. 129, No. 5, pp. 571–583, 2007. [5] C. W. Reynolds, “Flocks, Herds and Schools: A Distributed Behavioral Model,” Computer Graphics, Vol. 21, No. 4, pp. 25–34, 1987. [6] T. Vicsek, A. Czirók, E. Ben-Jacob, I. Cohen and O. Shochet, “Novel Type of Phase Transition in a System of Self-Driven Particles,” Physical Review Letters, Vol. 75, No. 6, pp. 1226–129, 1995. [7] D. A. Paley, N. E. Leonard, R. Sepulchre, D. Grunbaum and J. K. Parrish “Oscillator Models and Collective Motion: Spatial patterns in the dynamics of engineered and biological networks,” IEEE Control Systems Magazine, Vol. 27, No. 4, 2007, pp.89-105. [8] R. Olfati-Saber, J. A. Fax and R. M. Murray, “Consensus and Cooperation in Networked Multi-Agent Systems,” Proc. of the IEEE, Vol. 95, No. 1, 2007. [9] W. Ren and R. W. Beard, Distributed Consensus in Multi-vehicle Cooperative Control, Springer, 2008. [10] A. Jadbabaie, J. Lin and A. S. Morse, “Coordination of Groups of Mobile Autonomous Agents Using Nearest Neighbor Rules,” IEEE Trans. on Automatic Control, Vol. 48, No. 6, pp. 988–1001, 2003.

15

[11] H. Tanner, A. Jadbabaie, and G. J. Pappas, “Flocking in Fixed and Switching Networks,” IEEE Trans. on Automatic Control, Vol. 52, No. 5, pp. 863–868, 2007. [12] N. Moshtagh and A. Jadbabaie, “Distributed Geodesic Control Laws for Flocking of Nonholonomic Agents,” IEEE Trans. on Automatic Control, Vol. 52, No. 4, pp. 681–686, 2007. [13] R. Olfati-Saber, “Flocking for Multi-Agent Dynamic Systems: Algorithms and Theory,” IEEE Trans. on Automatic Control, Vol. 51, No. 3, pp. 401–420, 2006. [14] D. J. Lee and M. W. Spong, “Stable Flocking of Multiple Inertial Agents on Balanced Graphs,” IEEE Trans. on Automatic Control, Vol. 52, No. 8, pp. 1469–1475, 2007. [15] A. Jadbabaie, N. Motee and M. Barahona, “On the Stability of the Kuramoto Model of Coupled Nonlinear Oscillators,” Proc. of the 2004 American Control Conference, pp. 4296–4301, 2004. [16] N. Chopra and M. W. Spong, “On Synchronization of Kuramoto Oscillators,” Proc. of the 44th IEEE Conference on Decision and Control, pp. 3916–3922, 2005. [17] N. Chopra and M. W. Spong, “Output Synchronization of Nonlinear Systems with Time Delay in Communication,” Proc. of the 45th IEEE Conference on Decision and Control, pp. 4986–4992, 2006. [18] E. W. Justh and P. S. Krishnaprasad, “Natural Frames And Interacting Particles in Three Dimensions,” Proc. of the 44th IEEE Conference on Decision and Control and European Control Conference 2005, pp. 2841–2846, 2005. [19] S. Nair and N. E. Leonard, “Stable Synchronization of Rigid Body Networks,” Networks and Heterogeneous Media, Vol. 2, No. 4, pp.595– 624, 2007. [20] R. Sepulchre, D.A. Paley and N. E. Leonard, “Stabilization of Planer Collective Motion: All-to-all communication,” IEEE Transactions on Automatic Control, Vol.52, No. 5, pp. 811–824, 2007. [21] R. Sepulchre, D. Paley and N. E. Leonard, “Stabilization of Collective Motion with Limited Communication,” IEEE Transactions on Automatic Control, Vol.53, No. 3, pp. 706–719, 2008. [22] L. Scardovi, A. Sarlette, and R. Sepulchre, “Synchronization and balancing on the N-tours,” Systems and Control Letters, vol. 56, no. 5, pp. 335–341, 2007. [23] L. Scardovi, N. Leonard, and R. Sepulchre, “Stabilization of Collective Motion in Three Dimensions,” Communications in Information and Systems, Brockett Legacy issue, Vol. 8, No. 3, pp. 473-500, 2008. [24] J. Cortes, S. Martinez, T. Karatas, F. Bullo, “Coverage Control for Mobile Sensing Networks,” IEEE Transactions on Robotics and Automation, Vol. 20, No. 2, pp. 243-255, 2004. [25] W. Li and C. G. Cassandras, “Distributive Cooperative Coverage Control of Sensor Networks,” Proc. of IEEE Conference on Decision and Control, pp. 2542-2547, 2005. [26] N. Chopra and M. W. Spong, “Passivity-Based Control of Multi-Agent Systems,” in Advances in Robot Control: From Everyday Physics to Human-Like Movements, S. Kawamura and M. Svnin, eds., pp. 107– 134, Springer, 2006. [27] M. Arcak, “Passivity as a Design Tool for Group Coordination,” IEEE Trans. on Automatic Control, Vol. 52, No. 8, pp. 1380–1390, 2007. [28] I.-A. F. Ihle, M. Arcak and T. I. Fossen, “Passivity-Based Designs for Synchronized Path Following,” Automatica, Vol. 43, No. 9, pp. 1508– 1518, 2007. [29] J. R. Lawton, and R. W. Beard, “Synchronized Multiple Spacecraft Rotations,” Automatica, Vol. 38, No. 8, 1359–1364, 2002. [30] W. Ren, “Synchronized Multiple Spacecraft Rotations: A Revisit in the Context of Consensus Building,” Proceedings of the 2007 American Control Conference, pp. 3174–3179, 2007. [31] A. Sarlette, R. Sepulchre and N. E. Leonard, “Autonomous Rigid Body Attitude Synchronization,” Automatica, Vol. 45, No. 2, pp. 572–577, 2009. [32] H. Bai, M. Arcak and J. T. Wen, “Rigid Body Attitude Coordination without Inertial Frame Information,” Automatica, Vol. 44, No. 12, pp. 3170–3175, 2008. [33] Y. Igarashi, T. Hatanaka, M. Fujita and M. W. Spong “Passivity-Based Attitude Synchronization in SE(3),” IEEE Trans. on Control Systems Technology, Vol. 17, No. 5, pp. 1119-1134, 2009. [34] Y. Igarashi, T. Hatanaka, M. Fujita and M. W. Spong, “Passivity-based Output Synchronization in SE(3),” Proc. of the 2008 American Control Conference, pp. 723–728, 2008. [35] M. Fujita, T. Hatanaka, N. Kobayashi, T. Ibuki and M. W. Spong, “Visual Motion Observer-Based Pose Synchronization: A Passivity Approach,” Proc. of the 48th IEEE Conference on Decision and Control and 28th Chinese Control Conference, pp. 2402–2407, 2009.


[36] S.-J. Chung, U. Ahsun, and J.-J. E. Slotine, “Application of Synchronization to Formation Flying Spacecraft: Lagrangian Approach,” AIAA Journal of Guidance, Control and Dynamics, Vol. 32, No. 2, pp. 512526, 2009. [37] S.-J. Chung, and J.-J. E. Slotine, “Cooperative Robot Control and Concurrent Synchronization of Lagrangian Systems,” IEEE Trans. on Robotics, Vol. 25, No. 3, pp. 686-700, 2009. [38] H. R. Karimi and P. Maass “Delay-Range-Dependent Exponential H∞ Synchronization of A Class of Delayed Neural Networks,” Chaos, Solitons & Fractals, Vol. 41, No. 3, pp. 1125-1135, 2009. [39] H.R. Karimi and H. Gao, “New Delay-Dependent Exponential H∞ Synchronization for Uncertain Neural Networks with Mixed Time Delays,” IEEE Trans. on Systems, Man, and Cybernetics, Part B: Cybernetics, Vol. 40, No. 1 pp. 173-185, 2010. ˇ [40] D. M. Stipanović, P. F. Hokayem, M. W. Spong and D. D. Siljak, “Cooperative Avoidance Control for Multiagent Systems,” Journal of Dynamic Systems, Measurement, and Control, Vol. 129, No. 5, pp. 699– 707, 2007. [41] G. Leitmann, “Guaranteed Avoidance Strategies,” Journal of Optimization Theory and Applications, Vol. 32, No. 4, pp. 569–576, 1980. [42] R. Murray, Z. Li and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994. [43] F. Bullo and A. D. Lewis, Geometric Control of Mechanical Systems, Springer, 2004. [44] M. Fujita, H. Kawai and M. W. Spong, “Passivity-based Dynamic Visual Feedback Control for Three Dimensional Target Tracking:Stability and L2-gain Performance Analysis,” IEEE Trans. on Control Systems Technology, Vol. 15, No. 1, pp. 40–52, 2007. [45] A. Jadbabaie, N. Motee and M. Barahona, “On the Stability of the Kuramoto Model of Coupled Nonlinear Oscillators,” Proc. of the 2004 American Control Conference, pp. 4296–4301, 2004. [46] A. Papachristodoulou and A. Jadbabaie, “Synchronization in Oscillator Networks with Heterogeneous Delays, Switching Topologies and Nonlinear Dynamics,” Proc. of the 45th IEEE Conference on Decision and Control, pp. 4307–4312, 2006. [47] M. W. Spong, S. Hutchinson and M. Vidyasagar, Robot Modeling and Control, John Wiley & Sons, Inc., 2006. [48] J. Hespanha, O. A. Yakimenko, I. I. Kaminer and A. M. Pascoal, “Linear Parametrically Varying Systems with Brief Instabilities: An Application to Vision/inertial Navigation,” IEEE Trans. on Aerospace and Electronics Systems, Vol. 40, No. 3, pp. 889–900, 2004. [49] L. Moreau, “Stability of Multiagent Systems With Time-Dependent Communication Links,” IEEE Trans. on Automatic Control, Vol. 50, No. 2, pp. 169–182, 2005. [50] A. Astolfi, “Exponential Stabilization of a Wheeled Mobile Robot via Discontinuous Control,” Journal of Dynamic Systems, Measurement, and Control, Vol. 121, No. 1, pp. 121–125, 1999. [51] D. V. Dimarogonas and K. J. Kyriakopoulos. “On the Rendezvous Problem for Multiple Nonholonomic Agents,” IEEE Trans. on Automatic Control, Vol. 52, No. 5, pp. 916–922, 2007. [52] T. Hatanaka, T. Ibuki, A. Gusrialdi, M. Fujita, “Coverage Control for Camera Sensor Networks: Its Implementation and Experimental Verification,” Proc. of the 17th Mediterranean Conference on Control and Automation, pp. 446-451, 2009. [53] R. A. Horn and C. R. Johnson, Matrix Analysis, Cambridge University Press, 1985. [54] H. K. Khalil, Nonlinear Systems, Third Edition, Prentice Hall, 2002.

PLACE PHOTO HERE

Takeshi Hatanaka received B.Eng. degree in informatics and mathematical science, M.Inf. and Ph.D. degrees in applied mathematics and physics all from Kyoto University, Japan in 2002, 2004 and 2007, respectively. From 2006 to 2007, he was a research fellow of the Japan Society for the Promotion of Science at Kyoto University. He is currently an assistant professor in the Department of Mechanical and Control Engineering, Tokyo Institute of Technology, Japan. His research interests include mobile sensor networks and constrained systems theory.

16

PLACE PHOTO HERE

Yuji Igarashi received the B.E. and M.E degrees in Department of Control and Systems Engineering from Tokyo Institute of Technology in 2006. He is currently is with the Advanced Technology R&D Center, Mitsubishi Electric Cooperation, Japan.

Masayuki Fujita is a Professor with the Department of Mechanical and Control Engineering at Tokyo Institute of Technology. He received the Dr. of Eng. degree in Electrical Engineering from Waseda PLACE University, Tokyo, in 1987. Prior to his appointment PHOTO at Tokyo Tech, he held faculty appointments at HERE Kanazawa University and Japan Advanced Institute of Science and Technology. His research interests include passivity-based visual feedback, cooperative control, and robust/predictive control with its industrial applications. He is currently Vice President of the CSS Conference Activities and a member of the CSS Board of Governors. He serves a Control Division Vice Head/Head of the Society of Instrument and Control Engineers (SICE) and served a Director of SICE. He has served as the General Chair of the 2010 IEEE Multi-conference on Systems and Control. He has served/been serving as an Associate Editor for the IEEE Transactions on Automatic Control, the IEEE Transactions on Control Systems Technology, Automatica, Asian Journal of Control, and an Editor for the SICE Journal of Control, Measurement, and System Integration. He is a recipient of the 2008 IEEE Transactions on Control Systems Technology Outstanding Paper Award. He also received the 2010 SICE Education Award and the Outstanding Paper Awards from the SICE.

Mark W. Spong received the D.Sc. degree in systems science and mathematics in 1981 from Washington University in St. Louis. From 19842008 he was at the University of Illinois at UrbanaPLACE Champaign. Currently, he is Dean of Engineering PHOTO and Computer Science at the University of Texas HERE at Dallas and holder of the Lars Magnus Ericcson Chair in Electrical Engineering. Dr. Spong is Past President of the IEEE Control Systems Society, a Fellow of the IEEE and past Editor-in-Chief of the IEEE Transactions on Control Systems Technology. His recent awards include the IEEE Transactions on Control Systems Technology Outstanding Paper Award, the IROS Fumio Harashima Award for Innovative Technologies, the Senior Scientist Research Award from the Alexander von Humboldt Foundation, the Distinguished Member Award from the IEEE Control Systems Society, the John R. Ragazzini and O. Hugo Schuck Awards from the American Automatic Control Council,and the IEEE Third Millennium Medal. Dr. Spong’s research interests are in robotics and nonlinear control. He has published more than 250 technical articles in control and robotics and is co-author of four books.

Passivity-based Pose Synchronization in Three

Passivity-based Pose Synchronization in Three

Suggest Documents

New Synchronization Method for Three-Phase Three ... - IEEE Xplore

Synchronization Methods for Three Phase Distributed Power ...

Grid Synchronization of Three-Phase Converters ...

Adaptive coupled synchronization among three coupled chaos ...

Synchronization of Chaos in an Array of Three Lasers - CiteSeerX

Synchronization in f lickering of three-coupled candle flames - Nature

Evaluation of three pose estimation algorithms for model ... - TU Delft

Three-dimensional face pose estimation based on ... - Semantic Scholar

Gertrude Stein in Portraits: A Pose Is a Pose Is a Pose

Breathing synchronization in interconnected

Synchronization in baroclinic systems

Synchronization in Virtual Realities

synchronization in chaotic systems

Synchronization in Java

synchronization in chaotic systems

SYNCHRONIZATION IN DUBBING

Synchronization methods for three phase distributed ... - IEEE Xplore

Combination Synchronization of Three Identical or Different Nonlinear

Structural Basis of Cortical Synchronization. I. Three Types of ... - CerCo

Phase synchronization of three locally coupled chaotic ... - SLU

general coupling for mutual synchronization of three identical oscillators

Phase synchronization of three locally coupled chaotic ... - SLU

A Note on Synchronization Steps in Firing Squad Synchronization ...

SCHEDULE SYNCHRONIZATION IN PUBLIC ... - CiteSeerX