Variational Principles for Optimal Control of Left-Invariant Multi-Agent Systems with Asymmetric Formation Constraints

02/05/2018 ∙ by Leonardo Colombo, et al. ∙ KTH Royal Institute of Technology 0

We study an optimal control problem for a multi-agent system modeled by an undirected formation graph with nodes describing the kinematics of each agent, given by a left invariant control system on a Lie group. The agents should avoid collision between them in the workspace. This is accomplished by introducing appropriate potential functions into the cost functional for the optimal control problem, corresponding to fictitious forces, induced by the formation constraint among agents, that break the symmetry of the individual agents and the cost functions, and render the optimal control problem partially invariant by a Lie group of symmetries. Reduced necessary conditions for the existence of normal extrema are obtained using techniques from variational calculus on manifolds. The Hamiltonian formalism associated with the optimal control problem is explored through an application of Pontryagin's maximum principle for left-invariant systems where necessary conditions for the existence of normal extrema are obtained as integral curves of a Hamiltonian vector field associated to a reduced Hamiltonian function. By means of the Legendre transformation we show the equivalence of both frameworks. The discrete-time version of optimal control for multi-agent systems is studied in order to develop a variational integrator based on the discretization of an augmented cost functional in analogy with the Hamiltonian picture of the problem through the Hamilton-Pontryagin variational principle. Such integrator defines a well defined (local) flow to integrate the necessary conditions for local extrema in the optimal control problem. As an application we study an optimal control problem for multiple unicycles.



There are no comments yet.


page 1

page 2

page 3

page 4

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

1. Introduction

Multi-agent systems [28] have gained a lot of attention in the last decades due to the advances in communication, robotics and cooperative control of spacecraft, robotic manipulators as well as unmanned aerial and underwater vehicles. Symmetries in optimal control have been studied by several authors in the last decades [3], [10], [23], [24], [38] and the reduction by symmetries in optimal control problems has been a very active area of research for applications in robotics, aerospace engineering and locomotion among others (see e.g., [7], [22] and references therein).

Different approaches to formation control of multi-agent systems can be identified, e.g. as leader-follower [25], behavior-based [2] and rigid body type formations [30], [31], [33], [36]. We build on the last category by studying optimal control of formation problems for systems whose configurations evolves on a Lie group and including in our analysis rigid body dynamics.

Discrete Mechanics is, roughly speaking, a discretization of Geometric Mechanics theory. As a result, one obtains a set of discrete equations equivalent to the Euler-Lagrange equation but, instead of a direct discretization of the ODE, the latter are derived from a discretization of the base objects of the theory (the state space, the Lagrangian, etc). In particular, the derivation of variational integrators for Euler-Lagrange equations and Euler-Poincaré equations from the discretization of variational principles has received a lot attention from the systems and control community in the recent years [9], [27], [26], (and in particular for optimal control of mechanical systems [15], [21]). The preservation of the symplectic form and momentum map are important properties which guarantee the competitive qualitative and quantitative behavior of the proposed methods and mimic the corresponding properties of the continuous problem. That is, these methods allow substantially more accurate simulations at lower cost .

This work follows the research lines started in [23] for optimal control of left-invariant systems and [17] for coordination control of multiple left invariant agents, respectively, and also builds in previous developments for reduction of optimal control [4], [22], [23] by studying optimal control problems for multi-agent formations whose dynamics evolves on a Lie group of symmetries and the kinematics of each agent is given by a left-invariant system.

The problem studied in this work consists in finding the absolute configurations and control inputs for each agent, obeying the corresponding kinematics equations given by a left-invariant control system, as well as satisfying the formation constraints and minimizing the energy of the agents in the formation.

One of the aims of this paper, further than only solving the proposed optimal control problem, consists in introducing a new theoretical approach for the optimal coordinated motion of multi-agent systems with heterogeneous agents using variational principles, as is usual for a single agent whose dynamics is described by a mechanical system. The differential equations obtained represent necessary conditions for optimality and are obtained through three different variational principles.The first one, from a Lagrangian point of view, the second one from a Hamiltonian perspective, and the last one for discrete time systems inspired by the construction of geometric numerical integrators. Moreover, in the first variational principle the set of differential equations obtained for reduced necessary conditions for the existence of normal extrema gives rise to a set of equations that can not be solved directly with a numerical solver. We also propose a splitting in the equations to render such a system solvable.

In this work each agent is described by a drift-free kinematic control system on a Lie group and agents should satisfy the formation constraints to avoid collision with each other in the workspace. For this task, we introduce appropriate potential functions corresponding to fictitious forces (as for instance, a Coulomb potential), induced by the formation constraints, into the cost functional for the optimal control problem. Such potential functions are not invariant under the group of symmetries of the agents and therefore they break the symmetry of individual agents in the optimal control problem. For continuous time systems, the reduction of necessary conditions for the existence of normal extrema in the problem is described via Euler-Poincaré and Lie-Poisson type equations arising from the variational analysis while in the discrete time counterpart, the discrete necessary conditions are determined via discrete Lie-Poisson type equations [14].

As an application we study a minimum-energy problem for three unicycles and characterize the exact solution for the control law of one of the agents. To the best of authors’ knowledge, this is one of the first attempts where a formation constraint for a coordination motion of unicycles is expressed in absolute configurations on the Lie group (a different approach for relative configurations has been studied in [17] and [18]), allowing to explore more the Lie group framework in formation problems with non-compact configuration spaces. This approach can be seen as a complement to the related literature for formation problems on Lie groups is the case of agents evolving on the Lie group of rotations where the constraint is written as the geodesic distance between two points (since is compact and therefore a complete manifold such a distance is well defined) and the use of Rodrigues’ formula allows the use of trackable formation constraints. Moreover, the optimization problem considered in [17] and [18] is based on minimizing the strength of interactions by using a coupling parameter among particles while our optimization problem is a minimum-energy problem avoiding collisions among agents by using artificial potentials created to simulate a fictitious repulsion among them in the configuration space inspired by the approach given in [19] for robotic manipulators.

The main results of this work are given in Theorem 4.1, Theorem 5.3, Theorem 6.1, Proposition 4.1, and Proposition 6.1. The main contributions of this work are: (i) the introduction of the class of left-invariant multi-agent formations, where agents and formation constraints evolves on a Lie group of symmetries. This approach gives rise to coordinate-free expressions for the dynamics describing optimality conditions and given that agents are left invariant, there is a globalization of solutions; (ii) the reduction by symmetries of necessary conditions for optimality based on the invariance of each agent under a suitable symmetry in the cost function (Euler-Poincaré reduction) and the development of a splitting in the dynamics of the optimal control problem among the generators of the control distribution and its complementar; (iii) the reduction by symmetries of necessary conditions for optimality based on the Hamiltonian structure arising in the problem (Lie-Poisson reduction); (iv) the derivation of geometric numerical methods based on discrete mechanics and variational integrators (Hamilton-Pontryagin integrators) for the discrete-time optimal control problem of left-invariant multi-agent systems.

The structure of the paper is as follows: Section II introduces reduction by symmetries, Euler-Poincaré and Lie-Poisson equations. Section III introduces the left-invariant kinematic multi-agent control system and the formulation of the optimal control problem for multiple agents. In Section IV we study Euler-Poincaré and Lie-Poisson reduction of necessary conditions by using a variational framework and splitting the dynamics to find a solvable system of equations. The Hamiltonian formalism associated with the optimal control problem by applying Pontryagin’s maximum principle for left-invariant systems for the derivation of necessary conditions for the existence of normal extrema is given in Section V where we also establish the equivalence between the Lagrangian and Hamiltonian framework. The discrete-time version of the optimal control for multi-agent systems is studied in Section VI by discretizing a suitable cost functional in the spirit of discrete mechanics, and were we study some qualitative properties for the discrete flow. An application to the optimal control problem of three unicycles is studied in Section VII. We conclude the work by commenting directions of future research in Section VIII.

2. Preliminaries

In this section we introduce the review material we will use along the work. For a further covering of the topics see [3] (Chapter ), [13] (Chapters -), and [29] (Appendix A).

2.1. Mechanics on manifolds

Let be the configuration space of a mechanical system, a differentiable manifold of dimension with local coordinates . Let be the tangent bundle of , locally described by positions and velocities for the system with . Let be its cotangent bundle, locally described by positions and momentum for the system with . The tangent bundle at a point is denoted as and the cotangent bundle at a point is denoted as .

The dynamics of the mechanical system is determined by a Lagrangian function given by where is the kinetic energy and the potential energy. The corresponding equations of motion describing the dynamics of the system are given by the Euler-Lagrange equations , a system of second-order differential equations.

Given a tangent vector , and the set of real valued smooth functions on , denotes how tangent vectors acts on functions on .

Given a differentiable function with a smooth manifold, the pushforward of at is the linear map satisfying for all and . The pullback of at is the dual map satisfying


for all and , where denotes how tangent covectors acts on tangent vectors.

A differential -form (i.e., is a

-tensor) is called closed if

. If there exists such that for all , implies , is said to be non-degenerate. A differentiable manifold endowed with a closed and non-degenerated differential -form is called a symplectic manifold and denoted by . is called a symplectic structure on .

A diffeomorphism between two symplectic manifolds is called symplectomorphism of symplectic map if , where denotes the pullback of the -form by defined as , with and (see Definition in [13]).

The Poisson bracket of two functions on

is the bilinear, skew-symmetric operation



The canonical symplectic form on (i.e., when ) is given by ,where denotes the wedge product of differential forms (see Definition in [13]). Such a symplectic structure on induces a Poisson bracket given by

for all where satisfies for all . is called Hamiltonian vector field associated with . The contraction of by the vector field is defined by .

A Hamiltonian function is described by the total energy of a mechanical system, , giving rise to a dynamics on the , governed by Hamilton equations which correspond to the equations generated by the Hamiltonian vector field for , as a solution to the equation

locally described by , that is,

determining a set of

first order ordinary differential equations (see

[3] Section for more details).

2.2. Mechanics on Lie groups

If the configuration space is a Lie group and the system has a symmetry, one can take advantage of it to reduce the degrees of freedom of the system and work on a lower dimensional system reducing computational cost and avoiding singularities by working on a coordinate free framework in the associated Lie algebra of a Lie group.

Definition 2.1.

A Lie group is a smooth manifold that is a group and for which the operations of multiplication for and inversion, , are smooth.

Definition 2.2.

A symmetry of a function is a map such that . In such a case is said to be a -invariant function under .

Definition 2.3.

Let be a Lie group with identity element . A left-action of on a manifold is a smooth mapping such that , and for every , defined by is a diffeomorphism.

is a right-action if it satisfies the same conditions as for a left action except that .

We often use the notation and say that acts on . All actions of Lie groups will be assumed to be smooth.

Let be a finite dimensional Lie group and will denote the Lie algebra associated to defined as , the tangent space at the identity . Let be the left translation of the element given by for . Similarly, denotes the right translation of the element given by for . and are diffeomorphisms on and a left-action (respectively right-action) from to [13]. Their tangent maps (i.e, the linearization or tangent lift) are denoted by and , respectively. Similarly, the cotangent maps (cotangent lift) are denoted by and , respectively. It is well known that the tangent and cotangent lift are actions (see [13], Chapter ).

Let be a vector field on . The set denotes the set of all vector fields on . The tangent map shifts vectors based at to vectors based at . By doing this operation for every we define a vector field as for . A vector field is called left-invariant if for all . In particular for this means that a vector field is left-invariant if for . Note that if is a left invariant vector field, then .

Let for any a left action on ; a function is said to be invariant under the action , if , for any (that is, is a symmetry of ). The Adjoint action, denoted is defined by where . Note that this action represents a change of basis on the Lie algebra.

If we assume that the Lagrangian is -invariant under the tangent lift of left translations, that is for all , then it is possible to obtain a reduced Lagrangian , where

The reduced Euler–Lagrange equations, that is, the Euler–Poincaré equations (see, e.g., [3], [13]), are given by the system of first order ode’s


where , is the co-adjoint operator defined by for all with the adjoint operator given by , where denotes the Lie bracket of vector fields on the Lie algebra , and where denotes the so-called natural pairing between vectors and co-vectors defined by for and where is understood as a row vector and a column vector. For matrix Lie algebras (see [13], Section pp. for details).

Using this pairing between vectors and co-vectors and (1), one can write a useful relation between the tangent and cotangent lifts


for , and .

The Euler–Poincaré equations together with the reconstruction equation are equivalent to the Euler–Lagrange equations on . By assuming that the reduced Lagrangian is a global diffeomorphism (i.e. is hyper-regular), then one can obtain the reduced Hamiltonian given by where one uses the Legendre transformation given by and since is hyper-regular then can be defined as a function of by the implicit function theorem (see [13], Section , pp for details in the procedure). The Euler–Poincaré equations (2) can then be written as the Lie–Poisson equations (see, [3], [13]), which are given by .

3. Left-invariant kinematic multi-agent control system and problem formulation

3.1. Left-invariant kinematic multi-agent control system.

Let be Lie groups of dimension describing the configuration of heterogenous agents and their corresponding Lie algebras with describing the evolution of agent at time .

In the problem studied in this work the configuration space of each agent has the same Lie group structure. Note that the same configuration does not mean the same agent. For instance, each agent can have different mass and inertia values, and therefore agents can be heterogeneous.

Along this work, we assume that a multi-agent control system is modeled by an undirected (bidirectional) formation graph , describing the kinematics of each agent given by left invariant control systems on with together with the formation constraints.

Here denotes the set of vertices of the graph representing the communication topology in the multi-agent system where each vertex is a left invariant control system, that is, the kinematics of each agent is determined by


and the set denotes the set of edges of the graph, symmetric binary relations that link two agents, where , fixed, the time where agent should reach the desired position, and , the control input, is a curve on the Lie algebra of . Alternatively, the left-invariant control system (4) can be written as , where for each , the -tuple of control inputs take values in .

The set is given by (holonomic) formation constraints indexed by the edges set with . For each edge , is a function on defining the formation constraint between agents and . The constraint is enforced if and only if .

If for each , , with , then is given by

Therefore (4) is given by the drift-free kinematic left invariant control system


Left-invariant control systems (4) provide a general framework for a class of systems that includes control design for spacecraft and UAV motion. In general, the configuration space for these systems is globally described by a matrix Lie group making (4) a natural model for the controlled system. The Lie group framework gives rise to coordinate-free expressions for the dynamics describing the behavior of the system. When systems on Lie groups are left invariant, there is a globalization of solutions. That is, even if we exploit local charts to make small maneuvers, working in a framework of Lie groups allow us to move all over the configuration space without reformulating the controls. This is because the absolute position of the system can always be described as if it were the identity in the Lie group.

3.2. Problem formulation

Next, we are going to define an optimal control problem for the left-invariant multi-agent control system (5).

Along this work, we will denote and , where the Lie algebra structure of is given by with and We also denote , , , and the canonical projections from , , , and , respectively, over its -factor. These spaces and their corresponding projections are used in the following sections of the work to lift the dynamics in the product space and describe the dynamics for the optimal control of left-invariant multi-agent formation as a dynamical system for determining necessary conditions in the spirit of single agents. As commented in [33], when undirected formations are considered, each agent is equally responsible for maintaining constraints. Such a property permits to collect all the agent kinematics and the constraints by considering such product manifolds.

We assume that each agent occupies a disk of radius on . The quantity is chosen to be small enough so that it is possible to pack disks of radius on . We say that agents and avoids mutual collision if .

We want to find necessary conditions for the existence of normal extrema in a minimum-energy problem for the left-invariant multi-agent control system (5) where along their trajectory from a prescribed absolute initial state to a prescribed final absolute state not only minimize the energy of the complete networked system to achieve the desired final position, but also ensure that agents avoid collisions with each other in the workspace. This task can be done by introducing potential functions corresponding to fictitious forces into the cost functional for the optimal control problem, which are induced by the formation constraints.

Collision avoidance between the agents and is achieved by introducing the potential function with ; and . We assume that may not be a -invariant function for each (i.e., for some and in ) and that it is sufficiently regular for all .

Problem: Consider the following optimal control problem: find the absolute configurations and control inputs minimizing the cost functional


subject to and boundary values , , where and each cost function is smooth and -invariant for each under the left action of on given by , , that is, for all .

Remark 3.1.

For a given element , we denote , the action , and when there is no confusion we denote the vector valued left action of on given by for , .

Remark 3.2.

The restriction in the index in the second sum in (6) is to do not count twice the quantity of functions to avoid collision among agents (note that ) and make more simple the exposition. Other possibility might be to write in (6) the factor for .

Remark 3.3.

The cost functions are not related to collision avoidance between agents but only to the energy minimization performance. The potential functions used to avoid collision in the proposed approach are essentially centralized collision avoidance potentials as in [19]. However, this is not necessary. In particular, under some mild conditions one might consider other appropriated collision avoidance functions as for instance dipolar potential functions [34], [8], where each agent runs it’s own controller which has knowledge of the respective agent’s assigned target but ignores the targets of the others, and therefore, the potential function is local to each agent and becomes decentralized.

The problem studied in this work consists on finding necessary conditions for the existence of normal extrema in the optimal control problem (under the assumptions given above), taking advantage of the symmetries in the cost functional. To solve the proposed problem we extend, in a non-trivial fashion, the variational principle for Euler-Poincaré equations to the case of a multi-agent formation. The optimal control problem for left invariant systems studied in this work, for a single agent, has been studied in [22]. The inclusion of more agents satisfying the formation constraints in the setting proposed in [22] makes the problem more complicated to state and difficult to solve, compared with the single agent case. Also, in the proposed approach we include a decomposition of the dynamics describing the solution for the optimal control problem to transform the system into a solvable system of equations. Such decomposition was not necessary and thus not derived in [22] for the single agent case.

4. Reduced necessary conditions for the existence of normal extrema

As in [4], and [22] for the single agent case, the optimal control problem can be solved as a constrained variational problem by introducing the Lagrange multipliers with into the cost functional. Let , then , where the -tuple of Lagrange multipliers for each agent , takes values in .

We define the set of admissible trajectories by

and consider the extended Lagrangian given by

The following result gives rise to necessary conditions for the existence of reduced normal extrema in the optimal control problem.

Theorem 4.1.

If the cost functions are -invariant under the left action of on given by , for all among the set of admissible trajectories, the normal extrema for the optimal control problem (6) satisfy the Euler-Poincaré type equations


together with .

Remark 4.1.

Note that equations (7) looks like they are not Euler-Poincaré equations due to the -dependency, but we refer to them as Euler-Poincaré type equations, with some abuse, (also known in the literature as trivialized Euler-Lagrange equations [7], [13]) because they comes from a procedure of reduction by symmetries and if one considers the order in the way these equations must be solved, the first equation is an equation on the dual of the Lie algebra of . In practice, these equations should be applied in a backward sense with respect to the index , as illustrated below.

For instance, in the case of three agents (the one for which we apply the reduced equations in Section 7), . We start by computing the equations when . In this case and therefore we only need to solve the equation corresponding to the first two factors in (7) since the other part disappears for . Solving such an equation (we will show how such equation must be computed in Proposition 4.1) we obtain and , and therefore using and the reconstruction equation we obtain . Therefore, after this first step we have the evolution of and .

Next, we compute the equations when . In this case . Solving (7) coupled with , using the the configuration found for the last agent in the previous step, one is not able to solve the resulting equations and obtain the evolution for , and due to the interconnection term among the agents in , although we have the information to deal with from the previous stage. Then, we must couple the equations with the case , . That is, at this stage we need to go to step .

Solving such a system coupled also with we are able to find the evolution of , , , , and , giving rise to the motion of the agents in the workspace and the control inputs, satisfying the formation constraints and minimizing the cost functional (6).

Proof of Theorem 4.1: Given that for each , and , using (3),

Now, given that each is invariant under , it is possible to define the reduced Lagrangian as


Next, after obtaining the reduced Lagrangian we shown that for variations of vanishing, that is, and such that , , , and variations of , the variational principle


implies the constrained variational principle


for variations where , .

Given that each is -invariant under and both integrands are equal, and the variations of each , , induce and are induced by variations with (See [13] Section , pp 255). Therefore, if we choose variations such that (that is, ) for all , and the variational principle (9) holds, it follows that and hence the variational principle (9) implies the constrained variational principle (10).

Now, note that

where the first equality comes from the definition of variation of a function on a manifold, that is, for an arbitrary function in an arbitrary manifold, and the second one by replacing the variations by their corresponding expressions given before.

The first component of the previous integrand, after applying integration by parts twice, using the boundary conditions and the definition of co-adjoint action, results in

The second and third components of the previous integrand are given by

where we used the definition of left action in the first equality and (3) in the second one.

Therefore, , implies