Proceedings of the 46th IEEE Conference on Decision and Control New Orleans, LA, USA, Dec. 12-14, 2007

WePI20.16

Distributed Coordination of Dynamic Rigid Bodies Nima Moshtagh, Ali Jadbabaie, Kostas Daniilidis Abstract— This paper provides a design methodology to construct a set of distributed control laws for a group of rigid bodies moving in 3D space. The motion of the rigid bodies is restricted by a nonholonomic constraint that prohibits the agents from spinning around their velocity vectors. The connections between two seemingly different flocking control laws presented by Tanner et al. [1] and by Justh and Krishnaprasad [2] are studied, and it is shown that they are actually the same laws expressed in different coordinate systems. The former is expressed in the fixed world frame, whereas the latter is expressed in the moving body frame.

I. I NTRODUCTION The problem of distributed coordination of mobile agents has been studied in details in the literature [1]–[12]. There are generally two design methodologies for generating control laws that drive the multi-agent system into a formation. On the one hand, there is collection of literature [1], [8], [13] in which the control input, such as the acceleration vector of each agent, is a parameter that is expressed in some fixed global frame. On the other hand, there is a body of literature [5], [6], [14], [15] that develops distributed control laws for inputs, such as the rates of change of roll, pitch and yaw, which are expressed in the body frame of each agent. In this paper, the connections between two seemingly different formation control laws are studied. In particular, it is shown that the results presented by Tanner et al. [1] and by Justh and Krishnaprasad [2] are actually the same laws, but expressed in two different coordinate systems. In [2] the dynamics of each agent was described by the motion of its body frame. The body frame of each agent was represented by a natural Frenet frame that moved on the trajectory curve of that agent. They considered unit speed particles under the effect of gyroscopic forces that were perpendicular to the velocity vector. In [2] the authors provided a detailed analysis of the relative motion of two particles in the three dimensional space, and derived curvature controllers that achieved the desired rectilinear and circular formations. Their control laws depended only on relative positions and orientations (i.e. shape of the formation). They extended their results to the multi-agent case, but only for the case of all-toall communication network. Also, the proof of convergence for the multi-agent case was not presented in [2]. However, a detailed analysis of the distributed coordination of a group of dynamic agent, for both cases that the The authors are with the GRASP Laboratory, University of Pennsylvania, Philadelphia, PA, USA {nima, jadbabai,

kostas}@grasp.upenn.edu This work is supported in part by ONR Grant N0001406104 and ONR DURIP Grant N00014-07-1-0829 and ARO-MURI Grant W911NF-05-10381.

1-4244-1498-9/07/$25.00 ©2007 IEEE.

communication network is fixed and switching, is given by Tanner et al. [1]. They modeled each agent as point-mass particles and designed a control law for formation control of a group of mobile agents that drove the agents to some desired formation, while avoiding collisions among each other. The control input for each particle was its acceleration vector, which was expressed in the world frame. In this paper, we show that the flocking and coordination algorithm presented by Tanner et al. [1] for dynamic particles is actually the same as the one designed by Justh and Krishnaprasad in [2] with the difference that in the former the controller is expressed in a fixed world frame, whereas the controllers in the latter are expressed in the moving body frame of each agent. This paper is organized as follows. In Section II we present some known results on distributed coordination following the work of Tanner et al. [1], which gives us a set of control laws in the global frame. Then in Section III we develop the rigid body model that is used in this paper to describe the motion of each dynamic agent in space. In Section IV, a set of local controllers are derived by combining the results of the previous sections, and it turns out these local controllers are equivalent to the ones given by Justh and Krishnaprasad [2]. We also look into the circling formations of rigid bodies in Section V, and present both the body-frame inputs and the global-frame inputs that achieve the desired circular motion. Simulations and conclusions are presented in Sections VI and VII, respectively. II. D ISTRIBUTED C OORDINATION IN G LOBAL F RAME To study the problem of coordinated motion in a group of dynamical agents, we first express the dynamics in the fixed world frame {A}. By expressing the velocity and acceleration vectors in {A}, we can write the dynamics of each agent as a double integrator: r˙ i v˙ i

=

vi

=

ai ,

i = 1, . . . , N .

(1)

Now, consider a system of N agents with dynamics (1) in Rd , d ∈ {2, 3} moving with different velocities. Assume agents can communicate some information, say their velocities, with their neighbors. We can represent the neighboring relations among agents by a weighted graph. Definition 2.1 (Proximity Graph): The proximity graph G = {V, E, W} is a weighted graph consisting of: • a set of vertices V indexed by the set of mobile agents; • a set of edges E = {(i, j) | i, j ∈ V, and i ∼ j}; • a set of weights W, over the set of edges E.

1480

46th IEEE CDC, New Orleans, USA, Dec. 12-14, 2007

WePI20.16

25

8

20

6 4

15

U (| r|)

µ(| r|)

ij

z

2 0

10 5

−2

0

−4 −5 40

−6

20 −8 0

Fig. 1.

0.5

1

1.5

2

y

Artificial potential function Uij , and the norm of its gradient µ.

The neighborhood of agent i is defined by . Ni = {j|i ∼ j} ⊆ {1, . . . , N }\{i}.

(2)

The size of the neighborhood depends on the characteristics of each agent. We therefore assume that there is a predetermined radius which determines the neighborhood relationship. Let us formally define the coordinated motion or flocking in a multi-agent system: Definition 2.2 (Flocking): A group of mobile agents are flocking when all the agents attain the same velocity vectors, distances between the agents are stabilized, and no collisions among them occur. To ensure collision avoidance and cohesion of the formation, an inter-agent potential function [1], [13] can be defined. A control law from this artificial potential function results in simple steering behaviors known as separation and cohesion that govern how each agent maneuvers based on the relative position of its neighbors. The global minimum of this function is where all the agents are at the desired distances. It was shown in [1] that only if the underlying proximity graph is a spanning tree, the formation stabilizes at a state where the potential function is at the global minimum, and all the agents are at the desired distances. Whereas, in the general case, the multi-agent system reaches a stable state where the potential energy of the system is minimized (a local minimum). Next we formally define the notion of potential function used in this paper. The potential function Uij (|rij |) is a symmetric function of the distance |rij | = lij between agents i and j, and is defined as follows [1]: Definition 2.3 (Potential Function): Potential Uij is a differentiable, nonnegative function of the distance |rij | between agents i and j such that, • Uij → ∞ as |rij | → 0. • Uij attains its unique minimum when agents i and j are located at a desired distance. This definition ensures that minimization of the interagent potential functions leads to the desired cohesion and separation in the group. Agent i’s total potential is given by X Ui = Uij (|rij |) . j∈Ni

Fig. 2.

0

10 x

5

0

15

20

25

30

Coordinated motion (flocking) of point particles in the 3D space.

The requirements for Uij given in Definition 2.3 supports a large class of functions. Similar potential function as the following are used in both [1] and [2]: Uij =

d0 + log |rij | . |rij |

This choice of Uij provides an attractive force when an agent is moving away from the group, and a repulsive force when two agents get too close to each other. The gradient of this function is given by ¶ µ d0 1 rij = µ(|rij |)qij − ∇Uij = |rij | |rij | |rij |2 where q = r/|r| is the unit-length bearing vector between agent i and its neighbor j. See Figure 1 for the plots of the potential function Uij , and the norm of its gradient µ(|r|). Following the results of [1], we can design a control law for distributed coordination of the N-agent system. The acceleration vector ai is the control input of agent i, and it must have two components: aiv that controls the relative velocities; and aid that controls the spacing between the agents. aid must steer the agents apart to avoid collisions or pull them together if they are separating too far apart. It was proved by [1] and [8] that a control law of the form X X ai = aiv + aid = − (vi − vj ) − ∇Uij (3) j∈Ni

j∈Ni

can align the velocity vectors of all agents and stabilize the relative distances such that the potential energy of the N agent system is minimized. A sample trajectory of a group of dynamic agents, under the control laws (3), is plotted in Figure 2. So now, we have the following theorem: Theorem 2.4 ( [1]): Given a system of N agents with dynamics (1), if the proximity graph G of the system remains connected, then a control input of the form (3) will asymptotically drive the agents to a coordinated motion. It is shown in [1] that control law (3) minimizes the following energy function N

W =

1X T (v vi + Ui ) . 2 i=1 i

Next we will use the result of Theorem 2.4 to design a set of control inputs in the body frame of each agent.

1481

46th IEEE CDC, New Orleans, USA, Dec. 12-14, 2007

WePI20.16

III. R IGID B ODY DYNAMICS Suppose we have a group of N rigid bodies. Let the rotation matrix Ri = [xi yi zi ] represent the orientation of the body frame of agent i in a fixed world-frame {A}. Ri is also known as the attitude matrix of agent i (See Figure 3). Let us consider the dynamics of each rigid body as expressed in terms of the motion parameters of the body frame. The rate of change of the attitude matrix Ri is described by the equation (a) Parallel Transport Frame R˙ i = Ri ω bi T

where ωi = [ωix ωiy ωiz ] is the angular velocity expressed in the body frame {Bi } of agent i, and ω bi is its corresponding skew-symmetric matrix   0 −ωiz ωiy 0 −ωix  . (4) ω bi =  ωiz −ωiy ωix 0 The dynamics of each rigid-body agent is given by r˙ i v˙ i R˙ i

=

vi

=

ai

=

Ri ω bi .

We assume the velocity vector vi is along the third column of Ri , so we have: vi = Ri [0 0 vi ]T = vi Ri e3 .

(5)

Note that this choice of vi is only for simplifying the computations. Therefore, the rate of change of the velocity vi is given by: v˙ i = v˙ i Ri e3 + vi R˙ i e3 = αi zi + vi Ri ω b i e3

(6)

which by applying (4) becomes

v˙ i = αi zi − ωix vi yi + ωiy vi xi . In the end, the reduced dynamics of each agent is: r˙ i v˙ i v˙ i

= vi = αi zi − ωix vi yi + ωiy vi xi = αi .

(7)

System (7) is an underactuated system with control inputs αi (acceleration), ωix (yaw), and ωiy (pitch), which are all expressed in the body frame {Bi }.

Fig. 4. An airplane traverses a helix trajectory with (a) a Parallel Transport frame, and (b) a Frenet-Serret frame.

Thus, the problem considered in this paper can be stated as the following: Consider a group of rigid bodies described by the set of equations (7). Given that each agent can interact only with its nearest neighbors, design a set of distributed controllers in the local body frame that takes the group into flocking according to Definition 2.2. The choice of the body frame is important in our problem. At any moment, the orientation of the plane normal to the tangent vector depends on the history of the curve. For instance, depending on how we have framed the trajectory curve, two identical rigid bodies starting at the same configurations will end up with different final orientations. Figure (4) demonstrates the difference between (a) parallel transport framing, and (b) Frenet-Serret framing [16] as the plane traverses a helix trajectory. Now, consider again the rotation matrix Ri = [xi yi zi ]. Given that zi is aligned with the vector tangent to the trajectory, i.e. parallel to the velocity vector vi , we set the triad {xi , yi , zi } such that it forms a parallel transport frame [16], also known as natural Frenet frame [2]. The parallel transport frame is distinguished by the fact that it uses the smallest possible rotation at each point of the trajectory to align the current tangent vector with the next tangent vector. It can be shown that the body angular velocity of the plane with parallel transport frame is less than that with the FrenetSerret frame. Also, the roll is to be zero with parallel transport framing of the trajectory, as we will show next. Starting with with an arbitrary initial frame, one is essentially integrating a set of differential equations for the frame change around the trajectory. These differential equations for the parallel transport framing are given by x˙ i y˙ i z˙ i

Fig. 3.

The body frame of a rigid body (airplane).

(b) Frenet-Serret Frame

=

−k1 zi

(8)

=

−k2 zi

(9)

=

k1 xi + k2 yi

(10)

where k1 and k2 are the curvatures of the trajectory. Note that we can write the above equations in the more compact

1482

46th IEEE CDC, New Orleans, USA, Dec. 12-14, 2007

form:



0 [x˙ i y˙ i z˙ i ] = [xi yi zi ]  0 −k1

0 0 −k2



k1 k2  0

WePI20.16

and ωiy =

(11)

0 K= 0 −k1

0 0 −k2

ωix = −

ωiy = k1 ,

(18)

j∈Ni

 k1 k2  . 0

(12)

By comparing (4) and (12) we see that ωix = −k2 ,

1 X 1 X yi · vj + µ(|rij |)yi · qij . vi vi

ωiz = 0 .

Therefore, by choosing a parallel transport frame for the rigid body, we have eliminated the roll, or rotation about the velocity axis. Equivalently, the angular velocity vector is given by ωi = k1 yi − k2 xi , (13) which simply means that ωi is perpendicular to zi ( or vi ). Hence, we have shown that: Proposition 3.1: In a parallel transport frame the angular velocity vector is perpendicular to the linear velocity vector. As we will see in the next section, the control law for coordination of rigid bodies will satisfy the property stated in Proposition 3.1. IV. D ESIGN OF L OCAL C ONTROL L AWS Control law (3) is in terms of the velocity vector vi and vj that are expressed in a fixed global reference frame. To obtain equivalent body-frame inputs for each agent, as we saw in Section III, we have modeled each agent as a rigid body and have written the equations of motion in terms of inputs that are given in the local body frame. Systems (1) and (7) describe the dynamics of identical systems; one expresses the control inputs in the world frame and the other in the body frame. Given that the velocity vector of agent i is along the z-axis of the body frame, we showed in the previous Section that ai = v˙ i = αi zi − ωix vi yi + ωiy vi xi

j∈Ni

and

R˙ i = Ri K , 

(17)

j∈Ni

or equivalently where

1 X 1 X xi · vj − µ(|rij |)xi · qij vi vi

(14)

where xi and yi and zi form an orthonormal basis for the body-frame of agent i. Thus, in order to design control laws for αi , ωix and ωiy we set the two equations for v˙ i given by (3) and (14) to be equal: X X µ(|rij |)qij = αi zi −ωix vi yi +ωiy vi xi . (vj −vi )−

j∈Ni

The above terms for angular velocity components ωix and ωiy are equivalent to the rectilinear control inputs that were constructed by Justh and Krishnaprasad in [2]. For the case N > 2, it was conjectured in [2] that controllers of the above form will drive the multi-agent system to a coordinated motion. But here, we have systematically shown that the set of control laws presented in [2] are equivalent to that given by Tanner et al. in [1], which works for a multi-agent system with a switching proximity graph as well. The only difference is that (3) is an input for the double integrator model, whereas (16), (17) and (18) are the inputs for the rigid body model, and by a simple change of coordinate system we can go from one to the other. Note that because we have relaxed the unitspeed assumption, here there exists an acceleration controller αi that was not present in [2]. As we can see in the expressions of ωix , ωiy and αi , the control laws depend on the relative positions and velocities of the neighbors. In the body frame {Bi } the relative vectors are given by     xi · qij xi · vj i vj = yi · vj  , iqij = yi · qij  zi · qij zi · vj Thus, we can get a compact expression for the angular velocity vector ωi : 1 X 1 X i ( vi × ivj ) − µ(|rij |)(ivi × iqij ) (19) ωi = vi vi j∈Ni

j∈Ni

which is perpendicular to ivi = [0 0 vi ]T as expected from Proposition 3.1. Also, the acceleration input (16) can be written in terms of ivi and ivj : 1 X 1 X i i ( vi · vj −vi2 )− µ(|rij |)(ivi · bqij ) (20) αi = vi vi j∈Ni

j∈Ni

Thus, we have the following theorem:

j∈Ni

j∈Ni

(15) Take the inner product of the both sides by xi , yi and zi to get X X αi = (zi · vj − vi ) − µ(|rij |)zi · qij (16) j∈Ni

j∈Ni

Fig. 5. Relative position of two airplanes. qij is the unit bearing vector between agent i and its neighboring agent j.

1483

46th IEEE CDC, New Orleans, USA, Dec. 12-14, 2007

and solving for control inputs ωix , ωiy and αi we get: µ ¢ ¡ ¢¡ ¢ κ X ¡ yi · vj − qij · yi qij · vj ωix = vi j6=i ¶ ¡ ¢¡ ¢ + qij · yi qij · vi + µ(|rij |)(qij · yi ) (23)

4

3.5

3

2.5

y

WePI20.16

2

1.5

1

0.5

0

0

0.5

1

1.5

2

2.5

3

3.5

4

ωiy

4.5

=

x

Fig. 6.

−

The circular formation is achieved using the control law (22).

Theorem 4.1: Consider a system of N rigid bodies with reduced dynamics (7). If the proximity graph of the agents remains fixed and connected, by applying control inputs (19) and (20) the N -agent system flocks according to Definition 2.2. Interestingly, the angular velocity input ωi in (19) has the form of a cross-product steering law that was introduced in [17]. V. C ONTROL L AWS FOR C IRCULAR F ORMATIONS In addition to designing control laws for “rectilinear formations”, where at the equilibrium all agents have the same velocities, Justh and Krishnaprasad [2] designed control laws for “circular formations”, where the equilibrium of the system is where all agents are evenly distributed around a circle, as shown in Figure IV. Again, we start with the double integrator model in the world frame, and design a control law for circling formations. Consider the double integrator model of N point-mass particles: r˙ i v˙ i

= vi = ai ,

i = 1, . . . , N .

(21)

In order to achieve a circular motion, the relative velocities of the agents should be perpendicular to the relative position vector which is along the unit bearing vector qij for j ∈ {1, . . . , N }\{i} . The projection matrix that projects any vector onto the space perpendicular to q is given by P (q) = I − qqT where I is the identity matrix of the appropriate size. We propose the following controller for circular formation: X X ai = κ P (qij )(vi − vj ) − κ ∇Uij (22) j6=i

j6=i

where κ is a constant proportional gain. The equivalent control inputs in the body frame for circular formation of rigid bodies can be computed easily with similar method as used in the previous sections. By setting (22) equal to (14) X X κ (I − qij qTij )(vi − vj ) − κ ∇Uij j6=i

j6=i

= αi zi − ωix vi yi + ωiy vi xi

αi

= −

µ ¡ ¢ ¡ ¢¡ ¢ κ X − xi · vj + qij · xi qij · vj vi j6=i ¶ ¡ ¢¡ ¢ qij · xi qij · vi − µ(|rij |)(qij · xi ) (24) µ ¡ ¢ ¡ ¢¡ ¢ κ X − zi · vj + qij · vi qij · vj vi j6=i ¶ ¡ ¢2 qij · vi − µ(|rij |)(qij · vi ) (25)

The above expressions for ωix , ωiy are the ones derived in [2] as the curvatures of the trajectories. The first three terms in each case take care of the alignment of the velocities perpendicular to the bearing vector between the vehicles, and the last term guarantees collision avoidance and cohesion among the agents. Therefore, the (global) controller (22) and (local) controllers (23), (24), (25) are essentially equivalent to each other, with the only difference that they are expressed in different coordinate frames. Depending on the application one can use either one to achieve the desired circular formation. Note that the acceleration input αi was not present in the work of Justh and Krishnaprasad [2], because of the unit-speed assumption. VI. S IMULATIONS For simulation purposes an airplane is used as a rigid body.1 The dynamics of each airplane is given by (7) with control inputs being the body angular velocity and acceleration. The simulations are performed in MATLAB using the ode45 solver. At time t = 0, all agents start with random initial orientation and velocities as shown in Figure 7. The final formation of 5 aircraft is shown in Figure 8. All agents converge to a state where all the velocity vectors are the same and the distance between the agents is stabilized to the desired ones. As Figure 8 shows the final attitudes of agents could differ by a rotation about the velocity vector. The nonholonomic constraint of ωiz = 0, prohibits the airplanes from spinning around their velocity vectors while they have reached an equilibrium on the velocity consensus set. The trajectories of all agents are plotted in Figure 9. The circular formation can also be achieved in 3D using the control inputs (23), (24) and (25), as shown in Figure 10. In this case, the relative equilibria are parallel circles in the 3-D space. 1 The aircraft model is taken from the Mathworks FileExchange website http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do? objectId=5656

1484

46th IEEE CDC, New Orleans, USA, Dec. 12-14, 2007

Fig. 7.

Initial attitudes of five aircraft.

Fig. 8.

Final formation of five aircraft.

WePI20.16

Fig. 9.

The trajectories of all agents from t = 0 to t = 70.

Fig. 10.

VII. C ONCLUSIONS It is shown that by considering the dynamical model of a rigid body, we can design control laws for some local inputs such as the rate of pitch, roll or yaw of each agent that give us the desired coordinated motion. The connections between two seemingly different flocking control laws presented in [1] and [2] are studied, and it is shown that they are actually the same laws expressed in two different coordinate systems. A control law for circular/helical motion of particles was proposed in (22). The proof of convergence of this controller is an ongoing work. R EFERENCES [1] H. Tanner, A. Jadbabaie, and G. Pappas, “Flocking in fixed and switching newtorks,” IEEE Transaction of Automatic Control, vol. 52, pp. 863–868, 2007. [2] E. Justh and P. Krishnaprasad, “Natural frames and interacting particles in three dimensions,” in IEEE Conference on Decision and Control, 2005. [3] A. Jadbabaie, J. Lin, and A. S. Morse, “Coordination of groups of mobile autonomous agents using nearest neighbor rules,” IEEE Transactions on Automatic Control, vol. 48, no. 6, pp. 988–1001, June 2003. [4] Z. Lin, M. Brouke, and B. Francis, “Local control strategies for groups of mobile autonomous agents,” IEEE Transactions on Automatic Control, vol. 49, no. 4, pp. 622–629, April 2004. [5] R. Fierro, C. Belta, J. Desai, and V. Kumar, “On controlling aircraft formations,” in IEEE Conference on Decision and Control, 2001. [6] E. Justh and P. Krishnaprasad, “Equilibria and steering laws for planar formations,” Systems and Control letters, vol. 52, no. 1, pp. 25–38, May 2004.

4 aircraft in a circular formation.

[7] N. Moshtagh, A. Jadbabaie, and K. Daniilidis, “Vision-based distributed coordination and flocking of multi-agent systems,” in Proceedings of Robotics: Science and Systems, Cambridge, USA, June 2005. [8] R. Olfati-Saber, “Flocking for multi-agent dynamic systems: algorithms and theory,” IEEE Transaction of Automatic Control, March 2006. [9] D. Lee and M. Spong, “Stable flocking of inertial agents on balanced communication graphs,” in American Control Conference, 2006. [10] W. Ren and R. Beard, “Decentralized scheme for spacecraft formation flying via the virtual structure approach,” Journal of Guidance, Control, and Dynamics, 2004. [11] M. Mesbahi and F. Y. Hadaegh, “Formation flying of multiple spacecraft via graphs, matrix inequalities, and switching,” Journal of Guidance, Control, and Dynamics, March 2001. [12] R. Sepulchre, D. Paley, and N. E. Leonard, “Collective motion and oscillator synchronization,” in Lecture notes in Control and Information Sciences, 2005, pp. 189–206. [13] P. Ogren, E. Fiorelli, and N. Leonard, “Cooperative control of mobile sensing networks: Adaptive gradient climbing in a distributed environment,” IEEE Transaction of Automatic Control, vol. 49(8), pp. 1292– 1302, August 2004. [14] H. G. Tanner, A. Jadbabaie, and G. J. Pappas, “Coordination of multiple autonomous vehicles,” in IEEE Mediterranian Conference on Control and Automation, 2003. [15] N. Moshtagh and A. Jadbabaie, “Distributed geodesic control laws for flocking of nonholonomic agents,” IEEE Transaction on Automatic Control, vol. 52, april 2007. [16] A. J. Hanson, Visualizing Quaternions. Elsevier, 2006. [17] N. Moshtagh, A. Jadbabaie, and K. Daniilidis, “Vision-based control laws for distributed flocking of nonholonomic agents,” in IEEE International Conference of Robotics and Automation, Orlando, Florida, May 2006.

1485