
Citation: Luis M. Sandoval, Philip C. Goodell, Hector Gonzalez-Huizar, Munazzam Ali Mahar. Rayleigh wave group velocity model of the southeast flank of the Rio Grande Rift using Cross-Correlation[J]. AIMS Geosciences, 2018, 4(1): 1-20. doi: 10.3934/geosci.2018.1.1
[1] | Xueqi Sun, Yongqiang Fu, Sihua Liang . Normalized solutions for pseudo-relativistic Schrödinger equations. Communications in Analysis and Mechanics, 2024, 16(1): 217-236. doi: 10.3934/cam.2024010 |
[2] | Wang Xiao, Xuehua Yang, Ziyi Zhou . Pointwise-in-time α-robust error estimate of the ADI difference scheme for three-dimensional fractional subdiffusion equations with variable coefficients. Communications in Analysis and Mechanics, 2024, 16(1): 53-70. doi: 10.3934/cam.2024003 |
[3] | Yining Yang, Cao Wen, Yang Liu, Hong Li, Jinfeng Wang . Optimal time two-mesh mixed finite element method for a nonlinear fractional hyperbolic wave model. Communications in Analysis and Mechanics, 2024, 16(1): 24-52. doi: 10.3934/cam.2024002 |
[4] | Shengbing Deng, Qiaoran Wu . Existence of normalized solutions for the Schrödinger equation. Communications in Analysis and Mechanics, 2023, 15(3): 575-585. doi: 10.3934/cam.2023028 |
[5] | Enzo Vitillaro . Nontrivial solutions for the Laplace equation with a nonlinear Goldstein-Wentzell boundary condition. Communications in Analysis and Mechanics, 2023, 15(4): 811-830. doi: 10.3934/cam.2023039 |
[6] | Zhen Wang, Luhan Sun . The Allen-Cahn equation with a time Caputo-Hadamard derivative: Mathematical and Numerical Analysis. Communications in Analysis and Mechanics, 2023, 15(4): 611-637. doi: 10.3934/cam.2023031 |
[7] | Ho-Sik Lee, Youchan Kim . Boundary Riesz potential estimates for parabolic equations with measurable nonlinearities. Communications in Analysis and Mechanics, 2025, 17(1): 61-99. doi: 10.3934/cam.2025004 |
[8] | Mohamed Karim Hamdani, Lamine Mbarki, Mostafa Allaoui . A new class of multiple nonlocal problems with two parameters and variable-order fractional p(⋅)-Laplacian. Communications in Analysis and Mechanics, 2023, 15(3): 551-574. doi: 10.3934/cam.2023027 |
[9] | Cheng Yang . On the Hamiltonian and geometric structure of Langmuir circulation. Communications in Analysis and Mechanics, 2023, 15(2): 58-69. doi: 10.3934/cam.2023004 |
[10] | Siegfried Carl . Quasilinear parabolic variational-hemivariational inequalities in RN×(0,τ) under bilateral constraints. Communications in Analysis and Mechanics, 2025, 17(1): 41-60. doi: 10.3934/cam.2025003 |
Symmetries linked to Lie groups appear naturally in many control systems problems [1,2,3,4,5,6,7,8,9,10,11,12,13]. Numerical integrators evolving on Lie groups are commonly employed to improve its accuracy, as well as to avoid singularities by working with coordinate-free expressions in the Lie algebra associated to the Lie group, on which the system evolves. Typical tasks include trajectory tracking and estimation algorithms for the pose of mechanical systems.
Optimization problems on Lie groups [14] have applications in control engineering. Indeed, there are many examples of robotic systems that possess invariant quantities steaming from existing symmetries. Invariant quantities can be used as leverage to reduce the complexity of the equations, by projecting them into lower dimensional spaces. Techniques taking advantage of symmetries have been studied in [15,16,17,18,19,20], among many others, mainly for applications in robotic and aerospace engineering and, in particular, for spacecraft attitude control and underwater vehicles [21]. Most of the applications provided in the literature focus on the single-agent situation and only a few works explore symmetry reduction in a multi-agent scenario (see for instance [22,23,24,25]). In this work, we employ symmetry reduction to study optimal control problems with broken symmetry for multi-agent systems on Lie groups, while agents avoid collisions and obstacles in the configuration space.
In [26], the authors studied symmetry reduction in optimal control problems with broken symmetry for single-agent systems. In this work, we advance on the results of [26] by considering a multi-agent scenario. Hence, a generalization of the previous variational principle and reduction by symmetries performance are needed. The results in this paper are the Lagrangian/variational counterpart of those in [22]; we also develop a discrete-time version of the results. At the continuous-time side, we obtain the Euler–Poincaré equations from a constrained variational principle while after discretizing the variational principle in time, we obtain the discrete-time Lie–Poisson equations.
The approach in this work to obtain the reduced optimality conditions is a generalization of the one followed in [26], and is as follows. First, we consider a collection of free agents evolving on a Lie group G and an artificial potential V0, used to prevent collisions with a fixed obstacle, which is not symmetry invariant. At the same time, we consider a representation of G on a dual vector space W∗ and a graph G to describe interactions between neighboring agents. Though the artificial potential is not symmetry invariant, the interaction between neighbouring agents is, which is the standard situation when the interaction is done through a potential dependent on relative positions. Coupling the artificial potential a parameter depending on vectors in W∗, we obtain a symmetry invariant potential function under the action of G. Loosely speaking, the agents are coupled with vectors in W∗ that are acted by the representation. The associated action considered at this stage restores the full Lie group symmetry in the cost function from our optimal control problem and allows us to apply the semi-direct product reduction theory [3,27,28,29,30,31,32], to obtain the corresponding Euler-Poincaré system on the semi-direct product Lie algebra g⋉W∗ at each node. This gives rise to a new system that finds no analogs in classical reduced-order models in optimal control of mechanical systems.
The paper is organized as follows, in Section 2, we introduce some preliminaries about geometric mechanics on Lie groups and Lie group actions. In Section 3.1, we present the problem together with a motivating example. In Section 4, we study the Euler–Poincaré reduction of optimal control problems for left-invariant multi-agent control systems on Lie groups with partial symmetry breaking cost functions. Furthermore, we consider the discrete-time framework and obtain the discrete-time Lie–Poisson equations in Section 5. In Section 6, an example is considered to illustrate the theory. Finally, some concluding remarks are given in Section 7.
Consider a manifold Q to be the configuration space of a mechanical system and let it be differentiable of dimension d which can be described locally by coordinates q=(q1,…,qd). Denote by TQ the tangent bundle of Q, with local coordinates described by positions and velocities for the system, vq=(q1,…,qd,˙q1,…,˙qd)∈TQ with dim(TQ)=2d. Let T∗Q be its cotangent bundle, locally described by the positions and the momenta for the system, i.e., (q,p)∈T∗Q with dim(T∗Q)=2d. The tangent space at q∈Q has a vector space structure, and it is denoted as TqQ. The cotangent space at q∈Q is just the dual space of TqQ and is denoted as T∗qQ. The dynamics of a mechanical system is described by the equations of motion determined by a Lagrangian function L:TQ→R given by L(q,˙q)=K(q,˙q)−V(q), where K:TQ→R denotes the kinetic energy and V:Q→R the potential energy of the system. The equations of motion are given by the Euler-Lagrange equations ddt(∂L∂˙qi)=∂L∂qi,i=1,…,d, which determine a system of second-order differential equations. In case, the configuration space of the system is a Lie group, Euler-Lagrange equations can be reduced to a system of first-order ordinary differential equations.
Definition 2.1. Let G be a Lie group and Q a smooth manifold. A left-action of G on Q is a smooth map Φ:G×Q→Q, such that Φ(ˉe,g)=g and Φ(h,Φ(g,q))=Φ(hg,q) for all g,h∈G and q∈Q, where ˉe is the identity of the group G and the map Φg:Q→Q given by Φg(q)=Φ(g,q) is a diffeomorphism for all g∈G.
Definition 2.2. A function f:Q→R is called left invariant under Φg if f∘Φg=f for any g∈G.
For a finite dimensional Lie group G, its Lie algebra g is defined as the tangent space to G at the identity, g:=TˉeG. Let Lg:G→G be the left translation of the element g∈G given by Lg(h)=gh, where h∈G. Lg is a diffeomorphism on G and a left-action of G on G [33]. Its tangent map (i.e, the linearization or tangent lift) is denoted by ThLg:ThG→TghG. In a similar way, the cotangent map (cotangent lift), is denoted by (ThLg)∗, and is defined as the dual map of the tangent lift denoted by T∗hLg:T∗ghG→T∗hG, and determined by the relation ⟨(ThLg)∗(αgh),yh⟩=⟨αgh,(ThLg)yh⟩, yh∈ThG, αgh∈T∗ghG. As it is known, the tangent and cotangent lift of a Lie group action are Lie group actions as well. Here, ⟨⋅,⋅⟩:W∗×W→R with W a finite-dimensional vector space denotes the usual natural pairing between vectors and co-vectors, and defined by ⟨y,x⟩:=y⋅x for y∈W∗ and x∈W. For a matrix Lie algebra ⟨y,x⟩=yTx (see [33], Section 2.3). Using the natural pairing between vectors and co-vectors, for g,h∈G, y∈g∗ and x∈g, we write ⟨T∗gLg−1(y),TˉeLg(x)⟩=⟨y,x⟩.
Denote by ad∗:g×g∗→g∗, (ξ,μ)↦ad∗ξμ the co-adjoint operator, defined by ⟨ad∗ξμ,η⟩=⟨μ,adξη⟩ for all η∈g, where the ad:g×g→g denotes the adjoint operator on g given by the Lie-bracket, i.e., adξη=[ξ,η], ξ,η∈g. We also define the adjoint action of G on g, denoted by Adg:g→g and, in the case the Lie algebra is a matrix Lie group, it is given by Adgχ:=gχg−1, where χ∈g, and the co-adjoint action of G on g∗, denoted by Ad∗g:g∗→g∗, and given by ⟨Ad∗gα,ξ⟩=⟨α,Adgξ⟩ with α∈g∗.
For q∈Q, the isotropy (or stabilizer or symmetry) group of Φ at q is given by Gq:={g∈G|Φg(q)=q}⊂G. Since the map Φ(⋅,q)=Φq:G→Q is a continuous, Gq=(Φq)−1(q) is a closed subgroup, and thus, a Lie subgroup of G (see [34] Sec. 9.3 for instance).
Example 1. Consider the special Euclidean group SE(2) of rotations and translations on the plane. Elements on SE(2) can be expressed by transformations of R2 of the form z↦Rz+r, with r∈R2 and R∈SO(2). This transformation can be represented by g=(R,r), for R=(cosθ−sinθsinθcosθ) and r=[x,y]T. The composition law is (R,r)⋅(S,s)=(RS,Rs+r) with identity element (I,0) and inverse (R,r)−1=(R−1,−R−1r). Under this composition rule, SE(2) has the structure of the semidirect product Lie group SO(2)⋉R2. Here, as usual in the literature, we denote by ⋉ the semidirect product of Lie groups.
The Lie algebra se(2) of SE(2) is determined by se(2)={(Ab00)|A∈so(2)≃R,b∈R2}. In the following, for simplicity, we write A=−aJ, a∈R, where J=(01−10). Therefore, we denote ξ=(a,b)∈se(2). The adjoint action of SE(2) on se(2) is given by Ad(R,r)(a,b)=(a,aJr+Rb) (see [35], pp. 153 for instance), so, Ad(R,r)−1(a,b)=(a,RT(b−aJr)).
Next, we provide the infinitesimal description of a Lie group action, that will be an important concept in the remainder of the paper.
Definition 2.3. Given a Lie group action Φ:G×Q→Q, for ξ∈g, the map Φexp(tξ):Q→Q is a flow on Q, where exp is the exponential map of G. The corresponding vector field on Q, given by ξQ(q):=ddt∣t=0Φexp(tξ)(q) is called the infinitesimal generator of the action corresponding to ξ.
Definition 2.4. Let us denote the set of vector fields on a Lie group G by X(G). A left invariant vector field is an element X of X(G) such that ThLg(X(h))=X(Lg(h))=X(gh) ∀g,h∈G.
Especially, for h=ˉe, we have that a vector field X is left-invariant if X(g)=TˉeLgξ for ξ=X(ˉe)∈g. As a consequence, left invariant vector fields are identified with its corresponding element of g. Thus, for every g∈G and ξ∈g, we define the left-invariant vector field as ξ(g):=TˉeLg(ξ).
Example 2. Consider the Euclidean Lie group Rn with the sum as group operation. For all g∈Rn, the left translation Lg is the usual translation on Rn, that is, Lg(h)=g+h, h∈Rn. So that, the tangent map to Lg is the identity map on Rn, that is, T0Lg=idT0Rn, where we are using that ThRn≃Rn for all h∈Rn, since Rn is a vector space. Therefore, left-invariant vector fields are constant vector fields, that is, X=v1∂∂x1+…+vn∂∂xn for v=(v1,…,vn)∈T0Rn and x=(x1,…,xn)∈Rn.
Consider a Lie group G, a vector space W and the representation of G on W, given by ρ:G×W→W, (g,v)↦ρg(v), which is a left action, and it is defined by the relation ρg1(ρg2(v))=ρg1g2(v), g1,g2∈G. Its dual is given by ρ∗:G×W∗→W∗, (g,α)↦ρ∗g−1(α), satisfying ⟨ρ∗g−1(α),v⟩=⟨α,ρg−1(v)⟩, and it is also a left action of G now on W∗.
The infinitesimal generator of the left action of G on W is ρ′:g×W→W, (ξ,v)↦ρ′(ξ,v)=ddt|t=0ρexp(tξ)(v). For every v∈W, consider the linear transformation ρ′v:g→W, ξ↦ρ′v(ξ)=ρ′(ξ,v) and its dual ρ′∗v:W∗→g∗, α↦ρ′∗v(α). The last transformation defines the momentum map JW:W×W∗→g∗, (v,α)↦JW(v,α):=ρ′∗v(α), such that for every ξ∈g, ⟨JW(v,α),ξ⟩=⟨ρ′∗v(α),ξ⟩=⟨α,ρ′v(ξ)⟩=⟨α,ρ′(ξ,v)⟩.
For ξ∈g, consider the map ρ′ξ:W→W, v↦ρ′ξ(v)=ρ′(ξ,v) and its dual ρ′∗ξ:W∗→W∗, α↦ρ′∗ξ(α), such that ⟨ρ′∗ξ(α),v⟩=⟨α,ρ′ξ(v)⟩. Then, this satisfies ⟨JW(v,α),ξ⟩=⟨α,ρ′(ξ,v)⟩=⟨α,ρ′ξ(v)⟩=⟨ρ′∗ξ(α),v⟩. See [33] and [34] for more details on the momentum map.
Example 3. Let W=g and ρ the adjoint representation of G on W, i.e., ρg=Adg, for any g∈G. So, for α∈W∗=g∗, ρ∗ is the coadjoint representation of G on W∗, i.e., ρ∗g=Ad∗g. We also have that the infinitesimal generator for the adjoint representation is ρ′ξ=adξ for any ξ∈W (see [33], Def. 6.4, pp. 225), and it follows that ⟨JW(x,α),ξ⟩=⟨α,adξx⟩=⟨α,−adxξ⟩=⟨−ad∗xα,ξ⟩, for x∈W=g, which gives JW(x,α)=−ad∗xα.
Similarly, if now W=g∗ and the coadjoint representation of G on W is ρ, i.e., ρg=Ad∗g−1, for any g∈G, then ρ′ξ=−ad∗ξ for any ξ∈g. So, if x,ξ∈g and α∈g∗, it follows that ⟨JW(α,x),ξ⟩=⟨−ad∗ξα,x⟩=⟨α,adxξ⟩=⟨ad∗xα,ξ⟩, which gives JW(α,x)=ad∗xα.
Denote, by N, a set consisting of s≥2 free agents, and by E⊂N×N the set describing the interaction between them. The neighboring relationships are illustrated by an undirected graph G=(N,E), where N is the set of vertices and E the set of edges for G. We further assume G is static and connected. For every agent i∈N the set Ni={j∈N:(i,j)∈E} denotes the neighbors of that agent. The agent i∈N evolves on an n-dimensional Lie group G, and its configuration is denoted by gi∈G. We denote by Gs and by TeGs=:gs the cartesian products of s copies of G and g, respectively, where e=(¯e,¯e,…,¯e) is the identity of Gs, and ¯e the identity element of G.
For each agent i∈N, there is an associated left-invariant control system described by the kinematic equations
˙gi=T¯eLgi(ui),gi(0)=g0i, | (3.1) |
where gi(t)∈C1([0,T],G),T∈R fixed, ui∈g is the control input and gi0∈G is considered as the initial state condition. Letting dimg=n, we can have as many basis for the Lie algebra as the number of agents. Thus, for each i∈N, we have g=span{e1,e2,…,en} and the control inputs may be described by coordinates ui=[u1i,u2i,…,umi]T, where ui(t)∈C1([0,T],g), with m≤n. Hence, the control input for each agent is given by ui(t)=ei0+m∑k=1uki(t)ek, where ei0∈g. Thus, the left-invariant control systems (3.1) for each agent i∈N can be written as
˙gi(t)=gi(t)(e0+m∑k=1uki(t)ek),gi(0)=g0i. | (3.2) |
Note that the class of control systems described by (3.2) belongs to the family of underactuated control systems.
Following with Example 1, consider the agents i∈N and j∈Ni represented as gk=(Rk,rk), k∈{i,j}. Note that g−1igj=(RTiRj,−RTi(ri−rj)), then, Adg−1igj(1,0)=(1,JRTi(rj−ri)). The inner product on se(2) is given by ⟨ξ1,ξ2⟩=tr(ξT1ξ2) for ξ1,ξ2∈se(2) and, hence, the norm is given by ||ξ||=√tr(ξTξ), for any ξ∈se(2). For ξ=(a,b)∈se(2) we can write the norm of ξ as ||(a,b)||=√2a2+bTb. Therefore, ||Adg−1igj(1,0)||2=2+|JRTi(rj−ri)|2=2+|rj−ri|2, where we have used that R,J∈SO(2) for the last equality. Hence, it follows that |ri−rj|=√||Adg−1igj(1,0)||2−2.
The previous computation shows that, if the interaction between agents is determined by a function depending on the distances between them, that is, Vij:G×G→R, is such that Vij(gi,gj)=V(|ri−rj|) for some V:R≥0→R; then, Vij is SE(2)-invariant, that is Vij(hgi,hgj)=Vij(gi,gj). An alternative reasoning of this invariance property has been shown in [36].
Next, suppose that we wish to write the distance from an arbitrary point r∈R2 to a fixed point x0∈R2 in terms of the adjoint action. Consider ξ0=(1,Jx0)∈se(2). Then, for any (R,r)∈SE(2), we have Ad(R,r)−1(1,Jx0)=(1,RTJ(x0−r)), and, therefore, ||Ad(R,r)−1(1,Jx0)||2=2+|x0−r|2. Next, assume we have an obstacle avoidance function V0i:SE(2)→R for each agent i∈N which can be written as V0i(Ri,ri)=V(|ri|) with V:R≥0→R. Note that under this assumption, V may be chosen arbitrarily. Then, V0i is not SE(2)-invariant, but it is SO(2) invariant, i.e., V0i(R0Ri,ri)=V0i(Ri,ri) for any R0∈SO(2). Note also that SO(2) is the isotropy group for the coadjoint action, that is, SO(2)≃{g∈SE(2)∣Adg(ξ0)=ξ0}, therefore, the obstacle avoidance potential functions V0i are invariant under the left action of the isotropy group.
In this situation, one can redefine the potential function V0i to make it SE(2)-invariant as follows. Consider x0=0, so, ξ0=(R0,x0)=(−J,0)∈se(2), and ‖Ad(Ri,ri)−1ξ0‖=2+|ri|2. Hence, V0i(Ri,ri)=V(|ri|)=V(√||Ad(Ri,ri)−1ξ0||2−2). This gives a motivation to define an extended obstacle avoidance function Vexti,0:G⋉W∗→R with G⋉W∗=SE(2)⋉se(2) as Vexti,0(gi,ξ):=V(√||Adg−1iξ||2−2).
Note that the extended obstacle avoidance function possesses an SE(2)-symmetry (i.e., Vexti,0 is invariant under a left action of SE(2)) since Vexti,0∘˜Φ(h,(g,ξ))=Vexti,0(Lhg,Adhξ)=Vexti,0(g,ξ), for any h∈SE(2), with left action ˜Φ given by
˜Φ:SE(2)×(SE(2)×se(2))→SE(2)×se(2),(h,(g,ξ))↦(Lh(g),Adh(ξ)). | (3.3) |
The problem under study consists on finding reduced necessary conditions for optimality in an optimal control problem for (3.1) (or equivalently (3.2)). These solution curves should minimize a cost function and prevent collisions among agents, while they should also avoid static obstacles in the workspace.
Problem (collision and obstacle avoidance): Find reduced optimality conditions on g(t)=(g1(t),…,gs(t))∈Gs and the controls u(t)=(u1(t),…,us(t))∈gs avoiding collision among the agents and obstacles (which will be defined shortly) in the workspace, and such that (g(t),u(t))∈Gs×gs minimize the following cost function,
min(g,u)s∑i=1∫T0(Ci(gi(t),ui(t))+V0i(gi)+12∑j∈NiVij(gi(t),gj(t)))dt, | (3.4) |
subject to the kinematics ˙gi(t)=T¯eiLgi(t)(ui(t)), boundary conditions g(0)=(g1(0),…,gn(0))=(g01,…,g0s) and g(T)=(g1(T),…,gs(T))=(gT1,…,gTs) with T∈R+ the final time, and under the following assumptions:
(i) There is a left representation ρ of G on a vector space V.
(ii) Ci:G×g→R are G-invariant functions for each i∈N (under a left action of G on G×g, which is given below) and are also differentiable almost everywhere.
(iii) Vij:G×G→R (collision avoidance potential functions) satisfying Vij=Vji are G-invariant functions under Φ, defined by
Φ:G×(G×G)⟶G×G,(g,(g1,g2))⟼(Lg(g1),Lg(g2)), | (3.5) |
i.e., Vij∘Φg=Vij, for any g∈G, that is, Vij(Lg(gi),Lg(gj))=Vij(gi,gj), for any (gi,gj)∈E, j∈Ni and they are also differentiable almost everywhere.
(iv) V0i:G→R (obstacle avoidance potential functions) are not G-invariant functions and they are also differentiables almost everywhere, for i∈N.
(v) The obstacle avoidance functions V0i depend on a parameter α0∈W∗. Hence, we can define the extended potential function as Vexti,0:G×W∗→R, with Vexti,0(⋅,α0)=V0i, by making the parameter evolve - due to the group action - with initial value α0.
(vi) The extended obstacle avoidance functions are G-invariant under ˜Φ, defined by
˜Φ:G×(G×W∗)⟶G×W∗,(g,(h,α))⟼(Lg(h),ρ∗g−1(α)), | (3.6) |
where ρ∗g−1∈W∗ is the adjoint of ρg−1∈W, i.e., Vexti,0∘˜Φg=Vexti,0, for any g∈G, or Vexti,0(Lg(h),ρ∗g−1(α))=Vexti,0(g,α) where α∈W∗.
(vii) The obstacle avoidance potential functions are invariant under the left action of the isotropy group
Gα0={g∈G∣ρ∗g(α0)=α0}. | (3.7) |
Note that G×g is a trivial vector bundle over G and, in order to respect assumption (ⅱ), we define the left action of G on G×g as follows,
Ψ:G×(G×g)⟶(G×g),(g,(h,u))⟼(Lg(h),u). | (3.8) |
We further assume that each Ci:G×g→R is G-invariant under (3.8), i.e., Ci∘Ψg=Ci, for any g∈G. In addition, each agent i∈N occupies a disk of radius ¯r on G. This radius is consider to be small enough in order that all agents can be packed on G and, hence the potential functions are well defined and feasible for d(gi(t),gj(t))>2¯r for all t, where d(⋅,⋅):G×G→R denotes a distance function on G.
We next study reduced optimality conditions for extrema for the OCP. We address the problem as a constrained variational problem and obtain the Euler-Poincaré equations that extremal must satisfy in Theorem 4.1 and Proposition 4.2.
The optimal control problem (3.4) can be resolved as a constrained variational problem by utilizing the Lagrangian multipliers λgi=T∗giLg−1i(λi)∈T∗giG, where λi∈C1([0,T],g∗) into the cost functional.
Let {e1,…,em,em+1,…,en} be a basis of g∗ dual of the basis {ei} of g. Then λi=n∑k=m+1λikek, where λik are the components of the vector λi in the given basis of the Lie algebra g. Thus, we define the Lagrangian L:Gs×gs×(T∗giG)s→R by
L(g,u,λ)=s∑i=1[Ci(gi(t),ui(t))+⟨λgi,T¯eiLgiui⟩+V0i(gi(t))+Vi(g)], | (4.1) |
where Vi:Gs→R,
Vi(g)=12∑j∈NiVij(gi(t),gj(t)) |
By assumption (v), the obstacle avoidance potential functions V0i:G→R depend on a parameter α0∈W∗, so we can extend {them} to Vexti,0:G×W∗→R by making the parameter evolve under the Lie group action with α(0)=α0, and, therefore, we can consider the extended Lagrangian function on Gs×gs×(T∗giG)s×W∗,
Lext(g,u,λ,α)=s∑i=1[Ci(gi(t),ui(t))+⟨λgi,T¯eiLgiui⟩+Vexti,0(gi,αi)+Vi(g)] |
where Lext(g,u,λ,α0)=L(g,u,λ).
By assumptions (ⅰ) to (ⅳ), and by taking advantage of the G-invariance of Ci, Vexti,0 and Vij (and so Vi), we can define the reduced extended Lagrangian ℓ:Gs−1×gs×(g∗)s×W∗→R by
ℓ(g,u,λ,α)=s∑i=1[Ci(ui)+⟨λi,ui⟩+Vexti,0(¯e,αi)+Vi(g)], |
defining g1 to be the identity ˉe1 on G. This is equivalent to consider the Lagrangian ˜Lext(h,u,λ,α)=Lext(ˉe1,h,u,λ,α) with h∈Gs−1. Then ℓ(g,u,λ,α) is obtained by considering a left action in each term of the sum. In fact, if Liext is each term in the sum from 1 to s in the definition of Lext, then
ℓ(g,u,λ,α)=n∑i=1Liext(Lg−1ig,u,T∗giLg−1iλgi,αi) |
under the assumption that g1=ˉe1 and αi=ρ∗gi(α). Note here the slight abuse of notation regarding the positions g. In the definition of the reduced Lagrangian g∈Gs−1, while in that of the Lagrangian Lext, g∈Gs.
Theorem 4.1. For s≥2, an extremal for the OCP (3.4) satisfies the following Euler-Poincaré equations
\begin{equation} \frac{d}{dt}\Big(\frac{\partial C_i}{\partial u_i}+\lambda_i\Big) = \mathit{\text{ad}}^*_{u_i}\Big(\frac{\partial C_i}{\partial u_i}+\lambda_i\Big) +\mathit{\boldsymbol{J}}_W\Big(\frac{\partial V_{i, 0}^{\mathit{\text{ext}}}}{\partial\alpha_i}, \alpha_i\Big)+\Theta^{i}_{1}\sum\limits_{k = 1}^{s}T^*_{\overline{e}}L_{g_i}\bigg(\frac{\partial V_{k}}{\partial g_i}\bigg), \end{equation} | (4.2) |
\begin{equation} \dot{\alpha_i} = \rho'^*_{u_i}(\alpha_i), \;\quad \alpha_i(0) = \rho^*_{g_i^0}(\alpha_0), \end{equation} | (4.3) |
where \mathit{\boldsymbol{J}}_W:T^{*}W\simeq W\times W^{*}\to \mathfrak{g}^{*} is the momentum map that corresponds to the left action of G on W , and it is defined through the left representation \rho of G on W , and where \Theta^{i}_{1} = 0 if i = 1 , otherwise \Theta^{i}_{1} = 1 .
Proof. Consider the variational equation
\delta\int_0^TL_{ext}(g(t), u(t), \lambda(t), \alpha)dt = 0, |
which holds for variations of g , that vanish at the endpoints, and u . Also, consider the constrained variational equation
\begin{equation} \delta\int_0^T\ell(g(t), u(t), \lambda(t), \alpha(t))dt = 0, \end{equation} | (4.4) |
that holds for variations of u_{i} and \alpha_{i} with \delta u_{i} = \dot{\eta}_{i}+ \text{ad}_{u_{i}}\eta_{i} and \delta \alpha_{i} = \rho'^*_{\eta_{i}}(\alpha_{i}) , where \eta_{i} is a path of \mathfrak{g}^s that vanishes at the endpoints, i.e. \eta_{i}(0) = \eta_{i} (T) = 0.
The two variational equations are equivalent since the cost functions C_i and the extended potential functions V_{i, 0}^{\text{ext}} are G -invariant, i.e. C_i\circ\Psi_g, \; \; V_{i, 0}^{\text{ext}}\circ\tilde{\Phi} = V_{i, 0}^{\text{ext}} and \langle\lambda_{g_i}, T_{\bar{e}_i}L_{g_i}u_i\rangle = \langle T^{*}_{g_i}L_{g_i^{-1}}\lambda_{g_i}, u_i\rangle = \langle\lambda_i, u_i\rangle . The variations \delta g_i of g_i induce and are induced by variations \delta u_i = \dot{\eta}_i+ \text{ad}_{u_i}\eta_i with \eta_i(0) = \eta_i(T) = 0 , where \eta_i = T_{g_i}L_{g_i}^{-1}(\delta g_i) and \delta u_i = T_{g_i}L_{g_i^{-1}}(\dot{g}_i) . Variations of \alpha_i are given by \delta\alpha_i = \rho'^*_{\eta_i}(\alpha_i) . Thus, we have
\begin{equation} \begin{split} \delta\int_0^T\ell(g, u, \lambda, \alpha)dt = & \sum\limits_{i = 1}^s \int_0^T \Big\langle\frac{\partial C_i}{\partial u_i}, \delta u_i\Big\rangle+ \langle\lambda_i, \delta u_i\rangle \\ &+\Big\langle\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \delta\alpha_i\Big\rangle+ \sum\limits_{k = 2}^{s}\Big\langle\frac{\partial V_i}{\partial g_k}, \delta g_k\Big\rangle dt. \end{split} \end{equation} | (4.5) |
Using the variations of u_i , which are given by \delta u_i = \dot{\eta}_i+ \text{ad}_{u_i}\eta_i , applying integration by parts and by the definition of the co-adjoint action the first two terms, yield:
\int_0^T\Big\langle-\frac{d}{dt}\Big(\frac{\partial C_i}{\partial u_i}+\lambda_i\Big) + \text{ad}^*_{u_i}\Big(\frac{\partial C_i}{\partial u_i}+\lambda_i\Big), \eta_i\Big\rangle dt. |
From the variations \delta\alpha_i = \rho'^*_{\eta_i}(\alpha_i) , the third term gives
\begin{equation*} \Big\langle\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \delta\alpha_i\Big\rangle = \Big\langle\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \rho'^*_{\eta_i}(\alpha_i)\Big\rangle = \Big\langle\alpha_i, \rho'_{\eta_i}\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}\Big)\Big\rangle = \Big\langle{{\bf{J}}}_W\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \alpha_i\Big), \eta_i\Big\rangle. \end{equation*} |
Taking into account that T(L_{g_i}\circ L_{g_i^{-1}}) = TL_{g_i}\circ TL_{g_i^{-1}} is equivalent to the identity map on TG_i and \eta_i = T_{g_i}L_{g_i^{-1}}(\delta g_i) , the fourth term can be written as
\begin{equation*} \begin{split} \sum\limits_{k = 2}^{s}\Big\langle\frac{\partial V_{i}}{\partial g_k}, \delta g_k\Big\rangle& = \sum\limits_{k = 2}^{s}\Big\langle\frac{\partial V_{i}}{\partial g_k}, (T_{\overline{e}}L_{g_k}\circ T_{g_k}L_{g_k^{-1}})(\delta g_k)\Big\rangle = \sum\limits_{k = 2}^{s}\Big\langle\frac{\partial V_{i}}{\partial g_k}, T_{\overline{e}}L_{g_k}(\eta_k)\Big\rangle\\ & = \sum\limits_{k = 2}^{s}\Big\langle T^*_{\overline{e}}L_{g_k}\bigg(\frac{\partial V_{i}}{\partial g_k}\bigg), \eta_k\Big\rangle.\\ \end{split} \end{equation*} |
Therefore, after performing a change of variables between indexes i and k in the fourth term, the above variational equation (4.4) yields
\begin{equation*} \begin{split} \frac{d}{dt}\Big(\frac{\partial C_i}{\partial u_i}+\lambda_i\Big) = & \text{ad}^*_{u_i}\Big(\frac{\partial C_i}{\partial u_i}+\lambda_i\Big) +{{\bf{J}}}_W\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \alpha_i\Big). \end{split} \end{equation*} |
for i = 1 . Otherwise,
\begin{equation*} \frac{d}{dt}\Big(\frac{\partial C_i}{\partial u_i}+\lambda_i\Big) = \text{ad}^*_{u_i}\Big(\frac{\partial C_i}{\partial u_i}+\lambda_i\Big) +{{\bf{J}}}_W\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \alpha_i\Big)+\sum\limits_{k = 1}^{s}T^*_{\overline{e}}L_{g_i}\bigg(\frac{\partial V_{k}}{\partial g_i}\bigg). \end{equation*} |
Finally, by taking the time derivative of \alpha_i = \rho_{g_i}(\alpha_0) , we have \dot{\alpha}_i = \rho_{u_i}^{\prime*}(\alpha_i) , together with \alpha(0) = \rho^{*}_{g_{0}^{i}}(\alpha_{0}^{i}).
Note that the above Euler-Poincaré equations (4.2) cannot, in fact, describe the motion properly because there are more unknowns than equations. In particular, observe that equations (4.2) together with (3.1) (or equivalently (3.2)), give rise to only two equations for the three unknown variables u_i , \lambda_i and g_i . However, we provide an additional structure to the Lie algebra, \mathfrak{g} , that allows one to decouple equations (4.2) into two equations. The next Proposition describes this process.
Proposition 4.2. If the structure of the Lie algebra permits a decomposition \mathfrak{g} = \mathfrak{r}\oplus\mathfrak{s} where \mathfrak{r} = {\rm{span}}\{e_1, \dots, e_m\} and \mathfrak{s} = {\rm{span}}\{e_{m+1}, \dots, e_n\} such that
\begin{equation} [\mathfrak{s}, \mathfrak{s}]\subseteq\mathfrak{s}, \quad [\mathfrak{s}, \mathfrak{r}]\subseteq\mathfrak{r}, \quad [\mathfrak{r}, \mathfrak{r}]\subseteq\mathfrak{s}, \end{equation} | (4.6) |
then the Euler-Poincaré equations of motion (4.2) are given by the following equations:
\begin{equation} \begin{split} \frac{d}{dt}\frac{\partial C_i}{\partial u_i} = & \mathit{\text{ad}}^*_{u_i}\lambda_i+{\bf{J}}_W\Big(\frac{\partial V_{i, 0}^{\mathit{\text{ext}}}}{\partial\alpha_i}, \alpha_i\Big)\bigg|_{\mathfrak{r}}+\Theta^{i}_{1}\sum\limits_{k = 1}^{s}T^*_{\overline{e}}L_{g_i}\bigg(\frac{\partial V_{k}}{\partial g_i}\bigg)\bigg|_{\mathfrak{r}}, \\ \dot{\lambda_i} = & \mathit{\text{ad}}^*_{u_i}\frac{\partial C_i}{\partial u_i}+{\bf{J}}_W\Big(\frac{\partial V_{i, 0}^{\mathit{\text{ext}}}}{\partial\alpha_i}, \alpha_i\Big)\bigg|_{\mathfrak{s}}+\Theta^{i}_{1}\sum\limits_{k = 1}^{s}T^*_{\overline{e}}L_{g_i}\bigg(\frac{\partial V_{k}}{\partial g_i}\bigg)\bigg|_{\mathfrak{s}}, \\ \end{split} \end{equation} | (4.7) |
where u_{i}\in \mathfrak{r} and the restrictions \big|_{\mathfrak{r}} and \big|_{\mathfrak{s}} give the projection onto \mathfrak{r}^* and \mathfrak{s}^* , respectively, with respect to the associated splitting of the dual space \mathfrak{g}^* = \mathfrak{r}^* \oplus \mathfrak{s}^* .
Remark 4. Note that this is the case for semisimple Lie algebras, they admit a Cartan decomposition, i.e., if \mathfrak{g} is semisimple, then \mathfrak{g} = \mathfrak{r}\oplus\mathfrak{s} such that [\mathfrak{r}, \mathfrak{r}]\subseteq\mathfrak{s}, \quad [\mathfrak{s}, \mathfrak{r}]\subseteq\mathfrak{r}, \quad [\mathfrak{s}, \mathfrak{s}]\subseteq\mathfrak{s}, \nonumber where the set \mathfrak{r} = \{x\in\mathfrak{g}\mid\theta(x) = -x\} is the -1 eigenspace of the Cartan involution \theta , whereas the set \mathfrak{s} = \{x\in\mathfrak{g}\mid\theta(x) = x\} is the +1 eigenspace of the Cartan involution \theta . Additionally, the Killing form is proven to be positive definite on \mathfrak{r} and negative definite on \mathfrak{s} (see, e.g., [15]). Thus, suitable candidates that satisfy the assumption of Proposition 4.2 are connected semisimple Lie groups. On the other hand, a Cartan decomposition determines a Cartan involution \theta (see, e.g., [37]). In particular, the proposed decomposition for the Lie algebra is not restrictive in the sense that the usual manifolds/work-spaces used in applications as SO(n) and SE(n) allow such a decomposition.
Proof. Given \mathfrak{g} = \mathfrak{r}\oplus\mathfrak{s} we get \mathfrak{g}^* = \mathfrak{r}^*\oplus\mathfrak{s}^* , where \mathfrak{r}^* = {\rm{span}}\{e^1, \dots, e^m\} and \mathfrak{s}^* = {\rm{span}}\{e^{m+1}, \dots, e^s\} . Thus, from (4.6) we have that \text{ad}^*_{\mathfrak{s}}\mathfrak{s}^*\subseteq\mathfrak{s}^*, \; \text{ad}^*_{\mathfrak{s}}\mathfrak{r}^*\subseteq\mathfrak{r}^*, \; \text{ad}^*_{\mathfrak{r}}\mathfrak{s}^*\subseteq\mathfrak{r}^*, \; \text{ad}^*_{\mathfrak{r}}\mathfrak{r}^*\subseteq\mathfrak{s}^* and given that u_{i}\in \mathfrak{r} , \frac{\partial C_i}{\partial u_i}\in\mathfrak{r}^* and \lambda_i\in\mathfrak{s}^* by definition we conclude that \text{ad}^*_{u_i}\frac{\partial C_i}{\partial u_i}\in\mathfrak{s}^* and \text{ad}^*_{u_i}\lambda_i\in\mathfrak{r}^*. Also, {\bf{J}}_W\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \alpha_i\Big) \in \mathfrak{g}^* and \sum_{k = 1}^{s}T^*_{\overline{e}_i}L_{g_i}\bigg(\frac{\partial V_{k}}{\partial g_i}\bigg)\in \mathfrak{g}^* hence, they have a decomposition into \mathfrak{r}^* and \mathfrak{s}^* . Thus, the equations (4.2) split into the following equations
\begin{equation} \begin{split} \frac{d}{dt}\frac{\partial C_i}{\partial u_i} = & \text{ad}^*_{u_i}\lambda_i+{\bf{J}}_W\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \alpha_i\Big)\bigg|_{\mathfrak{r}}+\Theta^{i}_{1}\sum\limits_{k = 1}^{s}T^*_{\overline{e}}L_{g_i}\bigg(\frac{\partial V_{k}}{\partial g_i}\bigg)\bigg|_{\mathfrak{r}}, \\ \dot{\lambda_i} = & \text{ad}^*_{u_i}\frac{\partial C_i}{\partial u_i}+{\bf{J}}_W\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \alpha_i\Big)\bigg|_{\mathfrak{s}}+\Theta^{i}_{1}\sum\limits_{k = 1}^{s}T^*_{\overline{e}}L_{g_i}\bigg(\frac{\partial V_{k}}{\partial g_i}\bigg)\bigg|_{\mathfrak{s}}, \\ \end{split} \end{equation} | (4.8) |
Remark 5. For the initial value problem guaranteeing a solution for the previous system of equations, we look for a solution to the equations with the initial condition u_i(0) = T_{g(0)}L_{g^{-1}(0)}(\dot{g}_i(0)) and the kinematic equation \dot{g}_i(t) = T_{\overline{e}}L_{g_i(t)}(u_i(t)) with g(0) = (g_1(0), \dots, g_s(0)) .
Remark 6. Assuming the reduced Lagrangian is hyperregular, we use the Legendre transformation and we can define the reduced Hamiltonian h: G^{s-1}\times (\mathfrak{g}^*)^s\times (\mathfrak{g}^*)^s\times W^*\to \mathbb{R} given by
h(g, \mu, \lambda, \alpha) = \langle \mu, u\rangle - \ell(g, u, \lambda, \alpha), |
where \mu = \frac{\partial \ell}{\partial u} = \left(\frac{\partial C}{\partial u}+\lambda\right)\in(\mathfrak{g}^*)^s . Under this assumption, the Euler-Poincare equations (4.2), (4.3) can be written as the Lie-Poisson equations (see, e.g., [22])
\begin{eqnarray} &&\dot{\mu_i} = \text{ad}^*_{u_i}\mu_i +{\bf{J}}_W\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \alpha_i\Big) +\Theta^i_1\sum\limits_{k = 1}^{s}T^*_{\overline{e}}L_{g_i}\Big(\frac{\partial U_{k}}{\partial g_i}\Big), \end{eqnarray} | (4.9) |
\begin{eqnarray} &&\dot{\alpha_i} = \rho'^*_{u_i}(\alpha_i), \;\quad \alpha_i(0) = \rho^*_{g_i^0}(\alpha_0). \end{eqnarray} | (4.10) |
In this section, we study the discrete-time reduction by symmetries for necessary conditions in the collision and obstacle avoidance optimal control problem. The goal is to construct a variational integrator based on the discretization of the augmented cost functional. Such integrator inherits discrete-time symmetries from its continuous counterpart and generates a well-defined (local) flow for reduced necessary conditions characterizing (local) extremal in the optimal control problem.
Given the set \mathcal{T} = \{t_k\in\mathbb{R}^{+}, \, t_{k} = kh\mid k = 0, \ldots, N\} , Nh = T , with T fixed (recall that T\in\mathbb{R}^{+} is the end point of the cost functional - see for instance equation (3.4)), a discrete trajectory for the agent i is determined by a set of N+1 points equally spaced in time, g^{0:N}_i = \{g^0_i, \ldots, g^{N}_i\} , where g^k_i\simeq g_i(kh)\in G , and h = T/N is the time step. The path between two adjacent points g_i^k and g_i^{k+1} must be given by a curve lying on the Lie group G . To construct such a curve we make use of a retraction map \mathcal{R}:\mathfrak{g}\to G .
Definition 5.1. A retraction map \mathcal{R}:\mathfrak{g}\to G is an analytic local diffeomorphism assigning a neighborhood \mathcal{O}\subset\mathfrak{g} of 0\in {\mathfrak g} to a neighborhood of the identity \overline{e}\in G .
The retraction map (see Figure 1) is used to express small discrete changes in the group configuration through unique Lie algebra elements given by u^k = \mathcal{R}^{-1}((g^{k})^{-1}g^{k+1})/h , where u^k\in\mathfrak{g} (see [38,39] for further details). That is, if u^{k} were regarded as an average velocity between g^{k} and g^{k+1} , then \mathcal{R} is an approximation to the integral flow of the dynamics. The difference \xi^{k, k+1}: = (g^{k})^{-1}\, g^{k+1}\in G , which is an element of a nonlinear space, can now be represented by a vector space element u^{k} . For the derivation of the discrete equations of motion, the right trivialized tangent retraction map will be used. It is the function d\mathcal{R}:\mathfrak{g}\times\mathfrak{g}\rightarrow \mathfrak{g} given by
\begin{equation} T\mathcal{R}(\xi)\cdot\eta = TR_{\mathcal{R}(\xi)}d\mathcal{R}_{\xi}(\eta), \end{equation} | (5.1) |
where \eta\in\mathfrak{g} and R:G\to G the right translation on G and T\mathcal{R}(\xi)\cdot\eta is the directional derivative of \mathcal{R} at \xi in the direction of \eta (see [38,39] for the derivation of such a map). Here we use the following notation, d\mathcal{R}_\xi: = d\mathcal{R}(\xi):\mathfrak{g}\rightarrow \mathfrak{g}. The function d\mathcal{R} is linear, but only on one argument.
Remark 7. The natural choice of a retraction map is the exponential map at the identity \overline{e} of the group G, \mbox{exp}_{\overline{e}}:\mathfrak{g}\rightarrow G . Bear in mind that, for a finite-dimensional Lie group, \mbox{exp}_{\overline{e}} is locally a diffeomorphism and gives rise to a natural chart [40]. Then, there exists a neighborhood U of \overline{e}\in G such that \mbox{exp}_{\overline{e}}^{-1}:U\rightarrow \mbox{exp}_{\overline{e}}^{-1}(U) is a local \mathcal{C}^{\infty}- diffeomorphism. For an element g\in G a chart is given by \Psi_{g} = \mbox{exp}_{\overline{e}}^{-1}\circ L_{g^{-1}}.
Generally, it is not an easy task to work with the exponential map since the differential of the exponential map involves power series expansions with iterated Lie-brackets. Consequently, it will be more convenient to use a different retraction map. More concretely, the Cayley map, which is usually used in numerical integration with matrix Lie-groups configurations (see [38,39] for further details), will provide to us a proper framework in the application shown in the next Section.
Next, we consider a discrete cost function to construct variational integrators in the same way as in discrete mechanics [41]. In other words, consider the continuous-time Lagrangian {\bf{L}}:G^s\times\mathfrak{g}^s\to\mathbb{R} defined by the cost functional (3.4), that is,
\begin{equation*} {\bf{L}}(g, u) = \sum\limits_{i = 1}^s\Big(C_i(g_i(t), u_i(t))+V_i^{0}(g_i)+\frac{1}{2}\sum\limits_{j\in \mathcal{N}_i}V_{ij}(g_i(t), g_j(t))\Big), \end{equation*} |
and for a given h > 0 , we define the discrete Lagrangian L_d:G^s\times \mathfrak{g}^s\to\mathbb{R} as an approximation of the cost functional (3.4) along each discrete segment between g^{k} and g^{k+1} = g^{k}\mathcal{R}(hu^{k}) , that is,
L_{d}(g^{k}, u^{k}) = h{\bf{L}}\left(\kappa(g^k, u^{k}), \zeta(g^k, u^{k})\right) \simeq \int_{kh}^{(k+1)h}{\bf{L}}(g, u)\, dt, |
where \kappa and \zeta are functions of (g^k, u^{k})\in G^s\times \mathfrak{g}^s which approximate the configuration g(t) and the control input u(t) , respectively, in that interval. In the following, for simplicity, \kappa will be the projection onto G^{s} and \zeta will be the projection onto \mathfrak{g}^{s} . Thus, we consider
\begin{equation} L_d(g^k, u^{k}) = h\sum\limits_{i = 1}^{s}\left(C_{i}(g_{i}^k, u_{i}^k) +V_{i}^{0}(g_{i}^{k}) + V_{i}(g^{k})\right). \end{equation} | (5.2) |
We remark that different choices of \kappa and \zeta could result in higher-order numerical methods, such as, using the middle point rule with \kappa(g^k, u^{k}) = g^{k}\mathcal{R}(\frac{h}{2}u^{k}) .
Next, we are going to define the optimal control problem for discrete-time systems and derive a variational integrator for L_{d}\colon G^s\times \mathfrak{g}^s\to \mathbb{R} , in a similar fashion as the variational equation presented in Theorem 4.1.
Problem: Consider the discrete-time optimal control problem for collision and obstacle avoidance of left-invariant multi agent control systems, which is given by finding the discrete configurations \{g^k\}_{k = 0}^{N} = \{(g_1^k, \ldots, g_s^k)\}_{k = 0}^{N} and discrete control inputs \{u^k\}_{k = 0}^{N} = \{(u_1^k, \ldots, u_s^k)\}_{k = 0}^{N} minimizing the discrete cost functional
\begin{equation} \min\limits_{(g^k, u^k)}\sum\limits_{i = 1}^{s}\sum\limits_{k = 0}^{N-1}h\left(C_i(g_{i}^k , u_{i}^k) + V_{i}^{0}(g_{i}^{k}) + V_{i}(g^{k}) \right) \end{equation} | (5.3) |
subject to g_i^{k+1} = g_i^k\mathcal{R}(hu_i^k) (i.e., a first order approximation of equation (3.1)) with given boundary conditions g^0 and g^N , where h > 0 denotes the time step, \mathcal{R}:\mathfrak{g}\to G is a retraction map, u_i(0) and u_i(T) are given, and each cost function C_{i}:G\times\mathfrak{g}\to\mathbb{R} , potential functions V_{i}^{0} and V_{i} satisfy properties (ⅰ) - (ⅶ).
The discrete-time optimal control problem (5.3) can be considered as a discrete constrained variational problem by introducing the Lagrange multipliers \mu_{i}^k\in \mathfrak{g} into the cost functional. Consider the augmented discrete Lagrangian \mathcal{L}_{d}:G^s\times G^s\times \mathfrak{g}^s\times (\mathfrak{g}^{*})^s\to\mathbb{R} given by
\begin{equation} \begin{split} \mathcal{L}_d (g^k, g^{k+1}, u^k, &\mu^k) = h\sum\limits_{i = 1}^{s}\Bigg(C_{i}(g_{i}^k, u_{i}^k) +V_{i}^{0}(g_{i}^{k}) \\ &\quad + V_{i}(g^{k}) +\Big{\langle}\mu^k_i, \frac{1}{h}\mathcal{R}^{-1}(\xi^{k, k+1}_i)-u_{i}^k\Big{\rangle} \Bigg) \end{split} \end{equation} | (5.4) |
where \xi^{k, k+1}_i = (g_i^k)^{-1}g_i^{k+1}\in G , for each i\in\mathcal{N} . Note that the last term in the augmented Lagrangian represents a first-order discretization of the kinematic constraint paired with a Lagrange multiplier in analogy with the variational equation presented in Section 4.
Now, extending the potential V_{i}^{0} , we obtain an extended Lagrangian \mathcal{L}_{ext, d}:G^s\times G^s\times\mathfrak{g}^s\times(\mathfrak{g}^{*})^s\times (W^{*})^{s}\to\mathbb{R} given by
\begin{equation} \begin{split} \mathcal{L}_{ext, d} & (g^k, g^{k+1}, u^k, \mu^k, \alpha) = h\sum\limits_{i = 1}^{s}\left(C_{i}(g_{i}^k, u_{i}^k) \right. \\ + & V_{i}^{0, ext}(g_{i}^{k}, \alpha_i) + \left. V_{i}(g^{k}) +\Big{\langle}\mu^k_i, \frac{1}{h}\mathcal{R}^{-1}(\xi^{k, k+1}_i)-u_{i}^k\Big{\rangle} \right), \end{split} \end{equation} | (5.5) |
which is invariant under the left action of G on G^s\times G^s\times \mathfrak{g}^s \times (\mathfrak{g}^{*})^s \times (W^{*})^s given by \tilde{\Phi}_{g}(h_{1}, h_{2}, u, \mu, \alpha) = (gh_{1}, gh_{2}, u, \mu, \rho_{g^{-1}}^{*}(\alpha)) by assumption (ⅵ). In particular, under assumptions (ⅳ)-(ⅵ), the extended discrete Lagrangian \mathcal{L}_{ext, d}(\cdot, \cdot, \cdot, \cdot, \alpha_0) = :\mathcal{L}_{ext, d, \alpha_0} is G_{\alpha_0} -invariant under \tilde{\Phi}_g .
The following result (Theorem 5.2) derives a variational integration for reduced optimality conditions for the discrete-time optimal control (5.3) in analogy with the results presented in Section 4. To derive the numerical algorithm, first we need the following result describing variations for elements on the Lie algebra and its relation with variations on the Lie group by using the retraction map, in addition to a property used in the proof of Theorem 5.2. Though it is a well-known result in the literature, we include it here to make the paper self-contained.
Lemma 5.1 (adapted from [38,39]). The following properties hold
(i)
\frac{1}{h}\delta\left(\mathcal{R}^{-1}(\xi^{k, k+1})\right) = \frac{1}{h}d\mathcal{R}^{-1}_{(hu^k)}(-\eta^k+\hbox{Ad}_{\mathcal{R}(hu^k)}\eta^{k+1}), |
where \eta^{k} = T_{g^k}L_{(g^k)^{-1}}(\delta g^k)\in\mathfrak{g} and \xi^{k, k+1} = (g^k)^{-1}g^{k+1} .
(ii)
(d\mathcal{R}^{-1}_{(-hu^k)})^{*}\mu^k = \mathit{\text{Ad}}^{*}_{\mathcal{R}(hu^k)}(d\mathcal{R}^{-1}_{(hu^k)})^{*}\mu^{k}, |
where \mu^k\in(\mathfrak{g}^{*}) and d\mathcal{R}^{-1} is the inverse right trivialized tangent of the retraction map \mathcal{R} defined in (5.1).
Theorem 5.2. Under assumptions (i)-(vii), an extremal for the discrete-time optimal control problem (5.3) satisfies the following equations
\begin{align} g_i^{k+1} = &g_i^k\mathcal{R}(hu_i^k), \end{align} | (5.6) |
\begin{align} \left(d\mathcal{R}^{-1}_{(hu_i^k)}\right)^{*}\mu_i^k = &\left(d\mathcal{R}^{-1}_{(-hu_i^{k-1})}\right)^{*}\mu_i^{k-1} + {\bf{J}}_W\left(h\frac{\partial V^{0, ext}_{i}}{\partial \bar\alpha_i^k}, \bar\alpha_i^k\right) \\ & + h\Theta_{1}^{i}\sum\limits_{l = 1}^{s} T_{\overline{e}}^{*}L_{g_i^k}\left(\frac{\partial V_{l}}{\partial g_i^k}\right), \end{align} | (5.7) |
\begin{align} \mu_i^k = &\left(\frac{\partial C_i}{\partial u^k_i}\right), \end{align} | (5.8) |
\begin{align} \bar\alpha_i^{k+1} = &\rho^{*}_{\mathcal{R}(hu_i^k)}(\bar\alpha_i^k), \;\; \bar\alpha_i^0 = \rho_{g_i^0}^{*}(\alpha_i^0), \end{align} | (5.9) |
for k = 1, \ldots, N-1 ; where \Theta^{i}_{1} = 0 if i = 1 , otherwise \Theta^{i}_{1} = 1 .
Proof. Since the cost functions and the potential functions satisfy assumptions (ⅰ) - (ⅶ), as in the continuous-time case, it is possible to induce the reduced augmented discrete Lagrangian \ell_{ext, d}:G^{s-1}\times G^s \times\mathfrak{g}^s\times(\mathfrak{g}^{*})^s\times (W^{*})^s\to\mathbb{R} as
\begin{align*} \ell_{ext, d}(g^{k}, \xi^{k, k+1}, u^k, \mu^k, \bar\alpha^{k}) = & h\sum\limits_{i = 1}^{s}(C_{i}(u_{i}^k) + V_{i}^{0, ext}(\bar\alpha^{k}_i) \\&+\Big\langle \mu_{i}^k, \frac{1}{h}\mathcal{R}^{-1}(\xi_{i}^{k, k+1}) -u_{i}^k\Big\rangle + V_{i}(g^{k}), \end{align*} |
where \bar\alpha^{k}_i = \rho_{g^{k}_i}^{*}(\alpha_i^0) for a fixed \alpha_i^0\in W^{*} satisfying \alpha_i^0 = \rho^*_{g_i^0}(\alpha_i^0) and, with a slight abuse of notation, C_{i}(u_{i}^k) = C_{i}(\overline{e}, u_{i}^k) and V_{i}^{0, ext}(\alpha^{k}_i) = V_{i}^{0, ext}(\overline{e}, \alpha^{k}_i) . Notice that, also here, g_{1}^{k} is set to be the identity element, so that we have g^{k}\in G^{s-1} .
As in the proof for Theorem 4.1, the technical part is to show that an extremal of the reduced variational equation
\begin{equation} \delta \sum\limits_{k = 0}^{N-1} \ell_{ext, d}(g^{k}, \xi^{k, k+1}, u^k, \mu^k, \bar\alpha^{k}) = 0 \end{equation} | (5.10) |
satisfies equations (5.6)-(5.8) for all variations of \mathcal{R}^{-1}(\xi^{k, k+1}) (induced by variations of g^k vanishing at the endpoints), u^k and \bar{\alpha}^k of the form \rho_{\eta^{k}}^{'*}(\bar\alpha^{k}) , where \eta^k\in\mathfrak{g}^s vanishes at the endpoints. Then, similarly as in the proof for Theorem 4.1, it follows that an extremal for the optimal control problem (5.3) satisfies the variational equation
\delta \sum\limits_{k = 0}^{N-1} \mathcal{L}_{d} (g^k, g^{k+1}, u^k, \mu^k) = 0, |
for all variations of g^k (vanishing at the endpoints), \mathcal{R}^{-1}(\xi^{k, k+1}) (induced by variations of g^k ) and u^k .
Note that
\begin{equation*} \begin{split} 0 & = \delta \sum\limits_{k = 0}^{N-1} \ell_{ext, d}(g^{k}, \xi^{k, k+1}, u^k, \mu^k, \bar\alpha^{k}) \\ & = \sum\limits_{k = 0}^{N-1}\sum\limits_{i = 1}^{s}h\left[ \Big\langle \frac{\partial C_{i}}{\partial u_{i}^{k}} - \mu_{i}^{k}, \delta u_{i}^{k} \Big\rangle + \Big\langle \frac{\partial V_{i}^{0, ext}}{\partial \bar\alpha^{k}}, \delta \bar\alpha^{k}\Big\rangle + \sum\limits_{l = 2}^{s}\Big \langle \frac{\partial V_{i}}{\partial g_{l}^{k}}, \delta g_{l}^{k} \Big\rangle \right.\\ & + \left. \Big\langle \mu_{i}^{k}, \frac{1}{h} d\mathcal{R}_{h u_{i}^{k}}^{-1}(-\eta_{i}^{k}+ \text{Ad}_{\mathcal{R}(h u_{i}^{k})}\eta_{i}^{k+1})\Big\rangle \right] \end{split} \end{equation*} |
where we used Lemma 5.1 to obtain the last term. Since variations \delta u_{i}^{k} are arbitrary, we obtain \mu_{i}^{k} = \frac{\partial C_{i}}{\partial u_{i}^{k}} . As for the second term, we have that
\begin{equation*} \Big\langle \frac{\partial V_{i}^{0, ext}}{\partial \bar\alpha^{k}}, \delta \bar\alpha^{k}\Big\rangle = \Big\langle \frac{\partial V_{i}^{0, ext}}{\partial \bar\alpha^{k}}, \rho_{\eta_{i}^{k}}^{'*}(\bar\alpha^{k})\Big\rangle = \Big\langle {\bf{J}}_W\left(\frac{\partial V_{i}^{0, ext}}{\partial \bar\alpha^{k}}, \bar\alpha^{k}\right), \eta_{i}^{k} \Big\rangle. \end{equation*} |
As we have show along the proof for Theorem 4.1, we have that
\begin{equation*} \sum\limits_{l = 2}^{s} \Big\langle \frac{\partial V_{i}}{\partial g_{l}^{k}}, \delta g_{l}^{k} \Big\rangle = \sum\limits_{l = 2}^{s} \Big\langle T^*_{\overline{e}}L_{g_l^{k}}\bigg(\frac{\partial V_{i}}{\partial g_l^{k}}\bigg), \eta_l^{k}\Big\rangle. \end{equation*} |
and the indexes might be interchanged. The last term to obtain equation (5.7) may be dealt with, using integration by parts in discrete-time, which is just rearranging the indexes, together with the second statement in Lemma 5.1 and the fact that \eta_{i}^{0} = \eta_{i}^{N} = 0 . Therefore,
\begin{equation*} \begin{split} \sum\limits_{k = 0}^{N-1}\sum\limits_{i = 1}^{s} & \Big\langle \mu_{i}^{k}, d\mathcal{R}_{h u_{i}^{k}}^{-1}(-\eta_{i}^{k}+ \text{Ad}_{\mathcal{R}(h u_{i}^{k})}\eta_{i}^{k+1})\Big\rangle = \\ & \sum\limits_{k = 1}^{N-1}\sum\limits_{i = 1}^{s} \Big\langle \left(d\mathcal{R}^{-1}_{(-hu_i^{k-1})}\right)^{*}\mu_i^{k-1} - \left(d\mathcal{R}^{-1}_{(hu_i^k)}\right)^{*}\mu_i^k, \eta_{i}^{k}\Big\rangle \end{split} \end{equation*} |
and the equations (5.6)-(5.8) follow.
Remark 8. Equations (5.6)-(5.8) are as a discrete approximation of the Lie-Poisson equations for the Hamiltonian version of the optimal control problem considered in [22]. The equation {\mu_i^k = \left(\frac{\partial C_i}{\partial u^k_i}\right)} represents the discrete time version of the reduced Legendre transformation and the equation g^{k+1}_i = g^k_i\mathcal{R}(hu^k_i) is the analogous of the reconstruction equation in the discrete time counterpart. These three equations are used to compute u_i^{k}, \mu_i^{k} and g^{k+1}_i given u_i^{k-1} , \mu_i^{k-1} , g^{k-1}_i and g^{k}_i from k = 1 to k = N-1 .
To compute the discrete-time reduced necessary condition for the optimal control problem (5.3) we must enforce boundary conditions given by the continuous-time quantities. More precisely, we must set
\begin{equation} \begin{split} \left(d\mathcal{R}^{-1}_{(hu_i^{0})}\right)^{*}\mu_i^0 = &\frac{\partial C_i}{\partial u_i}(u_i(0))+h\Theta_{1}^{i} \sum\limits_{l = 2}^{s} T_{\overline{e}}^{*}L_{g_i^0}\left(\frac{\partial V_{l}}{\partial g_i^0}\right) + h {\bf{J}}_W\left(\frac{\partial V_{i}^{0, ext}}{\partial \bar\alpha^{0}}, \bar\alpha^{0}\right), \\ \frac{\partial C_i}{\partial u_i}(u_i(T)) = &\left(d\mathcal{R}^{-1}_{(-hu_i^{N-1})}\right)^{*}\mu^{N-1}_i , \end{split} \end{equation} | (5.11) |
relating the momenta at the initial and final times, and use to transform boundary values between the continuous and discrete representation. They follow from the principle that any variation with free boundary points of the action (5.10) along a solution of equations (5.6)-(5.9) equals the change in momentum \langle \frac{\partial C_i}{\partial u_i}(u_i(T)), g_{i}^N \delta g_{i}^{N} \rangle - \langle \frac{\partial C_i}{\partial u_i}(u_i(0)), g_{i}^0 \delta g_{i}^{0} \rangle (see [39] for a discussion in the single agent case). Therefore, we must enforce
\begin{equation*} \delta \sum\limits_{k = 0}^{N-1} \ell_{ext, d}(g^{k}, \xi^{k, k+1}, u^k, \mu^k, \bar\alpha^{k}) = \langle \frac{\partial C_i}{\partial u_i}(u_i(T)), g_{i}^N \delta g_{i}^{N} \rangle - \langle \frac{\partial C_i}{\partial u_i}(u_i(0)), g_{i}^0 \delta g_{i}^{0} \rangle \end{equation*} |
where the variation is taken along a family of solutions of the discrete equations (5.6)-(5.9) whose boundary values are not fixed. By taking the variations of the discrete action, we eventually get a vanishing term corresponding to the fact that this family of sequences satisfies the discrete equations together with boundary terms multiplying \eta_{i}^{0} and \eta_{i}^{N} , and simplifying, to the above equations.
Remark 9. If we choose the midpoint rule to discretize the potential V_{i} , then we would obtain the following boundary conditions
\begin{equation*} \begin{split} \left(d\mathcal{R}^{-1}_{(hu_i^{0})}\right)^{*}\mu_i^0 = &\frac{\partial C_i}{\partial u_i}(u_i(0))+\frac{h}{2}\Theta_{1}^{i} \sum\limits_{l = 1}^{s} T_{\overline{e}}^{*}L_{g_i^0}\left(\frac{\partial V_{l}}{\partial g_i^0}\right) + h {\bf{J}}_W\left(\frac{\partial V_{i}^{0, ext}}{\partial \bar\alpha^{0}}, \bar\alpha^{0}\right), \\ \frac{\partial C_i}{\partial u_i}(u_i(T)) = &\left(d\mathcal{R}^{-1}_{(-hu_i^{N-1})}\right)^{*}\mu^{N-1}_i + \frac{h}{2}\Theta_{1}^{i} \sum\limits_{l = 1}^{s} T_{\overline{e}}^{*}L_{g_i^N}\left(\frac{\partial V_{l}}{\partial g_i^N}\right).\hfill\diamond \end{split} \end{equation*} |
The boundary condition g_s(T) for agent s is enforced by the relation
\begin{equation} \mathcal{R}^{-1}((g^N_s)^{-1}g_s(T)) = 0. \end{equation} | (5.12) |
Recalling that \mathcal{R}(0) = \overline{e} , this last expression just means that g^N_s = g_s(T) . Moreover, by computing recursively the equation g^{k+1}_s = g^{k}_s\mathcal{R}(hu_s^k) for k = 1, \ldots, N-1 , using that g_s^{0} = g_s(0) and \mathcal{R}(0) = \overline{e} , it is possible to translate the final configuration g_s^N in terms of u^k_s , such that there is no need to optimize over any of the configurations g^k_s . In that sense, (5.7) for i = s , l = 0 together with
\begin{equation} \mathcal{R}^{-1}\left[\left(\mathcal{R}(hu^{N-1}_s)\right)^{-1}\ldots\left(\mathcal{R}(hu^0_s)\right)^{-1}(g_s(0))^{-1}g_s(T)\right] = 0, \end{equation} | (5.13) |
form a set of (nN) -equations (since \dim\mathfrak{g} = n ) where nN unknowns are for u^{0:N-1}_s , denoting the entire sequence of controls \{u_{s}^{0}, \dots, u_{s}^{N-1}\} for each agent s .
The numerical algorithm to compute the reduced optimality conditions is summarized in Algorithm 1.
Algorithm 1 Reduced conditions for optimality |
1: Data: Lie group G , its Lie algebra \mathfrak{g} , cost functions C_i , artificial potential functions V_{ij} , V_{i, \hbox{ext}}^{0} , final time T , \# of steps N . 2: inputs: g_i(0) , g_i(T) , u_i(0) , u_i(T) , \alpha^0_i , u_{i}^{0} for all i = 1, \ldots, s and h = T/N . 3: for i = 1 \to s do 4: Fix g_{i}^{0} = g_{i}(0) and \alpha^0_i 5: solve (5.6) and (5.9) for k = 0 . 6: outputs: g^1_s , \alpha^1_s 7: for k = 1 \to N-1 do 8: for i = 1 \to s do 9: solve (5.6)-(5.9) subject to (5.11). 10: outputs: g_s^{0:N-1} , u_s^{0:N-1} , \mu_s^{0:N-1}, \alpha_s^{0:N-1} . 11: Compute g_s^{0:N-1} , u_s^{0:N-1} , \mu_s^{0:N-1}, \alpha_s^{0:N-1} subjected to (5.13). |
Note also that the exact form of equations (5.6)-(5.8) depends on the choice of \mathcal{R} . This choice will also effect the computational efficiency of the optimization framework in the case, the above equalities are imposed as constraints. For instance, in Section 6, we will employ the Cayley transform on the Lie group SE(2) as a choice of \mathcal{R} to write in a compact form the numerical integrator [38], [17], but another natural choice would be to employ the exponential map, as we explained in Section 5.1.
In this case study, we apply the proposed reduction by symmetry strategy to an optimal control for autonomous surface vehicles (ASVs). The configuration space whose elements determine the motion of each ASV is SE(2)\cong SO(2)\times \mathbb{R}^2 . An element g_i\in SE(2) is given by g_i = \begin{pmatrix} \cos\theta_i & -\sin\theta_i&x_i\\ \sin\theta_i & \cos\theta_i&y_i\\ 0 & 0 & 1 \end{pmatrix} , where (x_i, y_i)\in \mathbb{R}^2 represents the center of mass of a planar rigid body describing the ASV and \theta_i represents the angular orientation of the ASV. The control inputs, for each ASV, are given by u_i = (u_i^1, u_i^2) , where u_i^1 denotes the speed of the center of mass for the ASV and u_i^2 denotes the angular velocity of the ASV.
The kinematic equations for the multi-agent system are:
\begin{equation} \dot{x_i} = u_i^2\cos\theta_i, \; \; \dot{y_i} = u_i^2\sin\theta_i, \; \; \dot{\theta_i} = u_i^1, \, i = 1, \ldots, s. \end{equation} | (6.1) |
Using the notation of Example 1, the Lie algebra \mathfrak{se}(2) is identified with \mathbb{R}^2 through the isomorphism \begin{pmatrix} -a\mathbb{J}&b\\ 0 & 0 \end{pmatrix}\mapsto (a, b). The elements of the basis of the Lie algebra \mathfrak{se}(2) are {e_1 = \begin{pmatrix} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 0 \end{pmatrix}, e_2 = \begin{pmatrix} 0 & 0 & 1\\ 0 & 0 & 0\\ 0 & 0 & 0 \end{pmatrix}, e_3 = \begin{pmatrix} 0 & 0 & 0\\ 0 & 0 & 1\\ 0 & 0 & 0 \end{pmatrix}} ,
which satisfy [e_1, e_2] = e_3, \; [e_2, e_3] = 0, \; [e_3, e_1] = e_2 . Thus, the kinematic equations (6.1) take the form \dot{g_i} = g_iu_i = g_i(u_i^1e_1+u_i^2e_2) and give rise to a left-invariant control system on SE(2)^s\times \mathfrak{se}(2)^s . The inner product on \mathfrak{se}(2) is given by \langle\langle\xi_1, \xi_2\rangle\rangle: = tr(\xi_1^T\xi_2) for \xi_1, \xi_2\in \mathfrak{se}(2) and hence, the norm is given by ||\xi|| = \sqrt{tr(\xi^T\xi)}, for any \xi\in \mathfrak{se}(2) . The dual Lie algebra \mathfrak{se}(2)^* of SE(2) is defined through the dual pairing, \langle\alpha, \xi\rangle = tr(\alpha \xi) , where \alpha\in \mathfrak{se}(2)^* and \xi\in \mathfrak{se}(2) , hence, the elements of the basis of \mathfrak{se}(2)^* are {e^1 = \begin{pmatrix} 0 & \frac{1}{2} & 0\\ -\frac{1}{2} & 0 & 0\\ 0 & 0 & 0 \end{pmatrix}, e^2 = \begin{pmatrix} 0 & 0 & 0\\ 0 & 0 & 0\\ 1 & 0 & 0 \end{pmatrix}, e^3 = \begin{pmatrix} 0 & 0 & 0\\ 0 & 0 & 0\\ 0 & 1 & 0 \end{pmatrix}} .
Consider the cost function C_i(g_i, u_i) = \frac{1}{2}\langle u_i, u_i\rangle and the artificial potential function V_{ij}:SE(2)\times SE(2)\to \mathbb{R} given by {V_{ij}(g_i, g_j) = \frac{\sigma_{ij}}{2((x_i-x_j)^2+(y_i-y_j)^2-4\overline{r}^2)}} , where \sigma_{ij}\in \mathbb{R}_{\geq 0} and \overline{r} is the radius of the disk each agent occupies as defined at the end of Section Ⅲ. Consider a spherical obstacle with unit radius and without loss of generality let it be centered at the origin. Hence, consider the obstacle avoidance potential function V_i^0:SE(2)\to \mathbb{R} , {V_i^0(g_i) = \frac{\sigma_{i0}}{2(x^2+y^2-(\overline{r}+1)^2)}} , where \sigma_{i0}\in\mathbb{R}_{ > 0} .
Note that the obstacle avoidance potential functions are not SE(2) -invariant but SO(2) -invariant, so they break the symmetry. Using the norm of \mathfrak{se}(2) and for \overline{r} = 1 , V_{ij} and V_i^0 are equivalently given by {V_{ij}(g_i, g_j) = \frac{\sigma_{ij}}{2(|| \text{Ad}_{g_i^{-1}g_j}e_1||^2-6)}} and {V_i^0(g_i) = \frac{\sigma_{i0}}{2(|| \text{Ad}_{g^{-1}_i}e_1||^2-6)}} .
Let W = \mathfrak{se}(2)^* , so we define the extended potential functions V_{i, 0}^{\text{ext}}:SE(2)\times \mathfrak{se}(2)\to \mathbb{R} by V_{i, 0}^{\text{ext}}(g_i, \alpha) = \frac{\sigma_{i0}}{2(|| \text{Ad}_{g^{-1}_i}\alpha||^2-6)} , which are SE(2) -invariant under the action of \tilde{\Phi} given by (3.6), i.e. V_{i, 0}^{\text{ext}}\circ\tilde{\Phi} = V_{i, 0}^{\text{ext}}, for any g\in SE(2) . Since, W = \mathfrak{se}(2)^* we have {{\bf{J}}_W\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}, \alpha_i\Big) = \text{ad}^*_{\alpha_i}\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}\Big)} , and equations (4.2) and (4.3) yield
\begin{align*} \frac{d}{dt}\Big(u_i+\lambda_i\Big) = & \text{ad}^*_{u_i}\Big(u_i+\lambda_i\Big) + \text{ad}^*_{\alpha_i}\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}\Big)\\&+\Theta_1^i\sum\limits_{j\in\mathcal{N}_i}T^*_{\overline{e}}L_{g_i}\bigg(\frac{\partial V_{ij}}{\partial \theta_i}e^1+\frac{\partial V_{ij}}{\partial x_i}e^2+\frac{\partial V_{ij}}{\partial y_i}e^3\bigg), \end{align*} |
together with \dot{\alpha}_i = - \text{ad}_{u_i}\alpha_i and \alpha_i = \text{Ad}_{g_i^{-1}}\alpha_0 .
Note also that u_i = u_i^1e_1+u_i^2e_2 , \alpha_i = \alpha_i^1e_1+\alpha_i^2e_2+\alpha_i^3e_3 and \lambda_i = \lambda_3^ie^3 thus,
\begin{align*} ad^*_{u_i}(u_i+\lambda_i) & = \begin{pmatrix} 0&-\frac{u_i^2\lambda_3^i}{2}&0\\ \frac{u_i^2\lambda_3^i}{2}&0&0\\ u_i^1\lambda_3^i&-u_i^1u_i^2&0 \end{pmatrix}, \, \, \text{ad}_{u_i}\alpha_i = \begin{pmatrix} 0&0&-u_i^1\alpha_i^3\\ 0&0&u_i^1\alpha_i^2-u_i^2\alpha_i^1\\ 0&0&0 \end{pmatrix}, \\ & \\ \text{ad}^*_{\alpha_i}\Big(\frac{\partial V_{i, 0}^{\text{ext}}}{\partial\alpha_i}\Big) & = \begin{pmatrix} 0&0&0\\ 0&0&0\\ \Gamma_{i, 0}^{31}&\Gamma_{i, 0}^{32}&0 \end{pmatrix} = \frac{\sigma_{i0}\alpha_i^1}{(||\alpha_i||^2-6)^2}\begin{pmatrix} 0&0&0\\ 0&0&0\\ -\alpha_i^3&\alpha_i^2&0 \end{pmatrix}, \\ & \\ T^*_{\overline{e}}L_{g_i}\Big(\frac{\partial V_{ij}}{\partial g_i}\Big) & = \begin{pmatrix} 0&0&0\\ 0&0&0\\ \Gamma_{ij}^{31}&\Gamma_{ij}^{32}&0 \end{pmatrix} = \frac{-\sigma_{ij}}{((x_{ij})^2+(y_{ij})^2-4)^2}\begin{pmatrix} 0&0&0\\ 0&0&0\\ x_{ij}&y_{ij}&0 \end{pmatrix}, \end{align*} |
where x_{ij} = x_i-x_j, \; y_{ij} = y_i-y_j and { \text{Ad}_{g_i^{-1}}\alpha_0 = \begin{pmatrix} 0 & -1&x_i\sin\theta_i-y_i\cos\theta_i\\ 1 & 0&x_i\cos\theta_i+y_i\sin\theta_i\\ 0 & 0 & 0 \end{pmatrix}} .
Therefore, by applying Proposition 4.2 for \mathfrak{r} = \text{span} \{e_1, e_2\} and \mathfrak{s} = \text{span} \{e_3\} , Euler-Lagrange equations for the OCP (3.4) are
\begin{equation*} \dot{u}_i^1 = -\frac{u_i^2\lambda_3^i}{2}, \quad\dot{u}_i^2 = u_i^1\lambda_3^i+\Gamma_{i, 0}^{31}+\Theta_1^i\sum\limits_{j\in\mathcal{N}_i}\Gamma_{ij}^{31}, \, \, \dot{\lambda}_3^i = -u_i^1u_i^2+\Gamma_{i, 0}^{32}+\Theta_1^i\sum\limits_{j\in\mathcal{N}_i}\Gamma_{ij}^{32}, \end{equation*} |
with
\begin{array}{ll} \dot{\alpha}_i^1 = 0, & \alpha_i^1(0) = 1, \\ \dot{\alpha}_i^2 = u_i^1\alpha_i^3, & \alpha_i^2(0) = x_i^0\sin\theta_i^0-y_i^0\cos\theta_i^0, \\ \dot{\alpha}_i^3 = -u_i^1\alpha_i^2+u_i^2\alpha_i^1, & \alpha_i^3(0) = x_i^0\cos\theta_i^0+y_i^0\sin\theta_i^0. \end{array}
For the discrete-time setting, one would choose
\begin{equation*} C_{i}(g^{k}_i, u^{k}_i) = \frac{h}{2}\langle u^{k}_{ij}, u^{k}_{ij}\rangle, \ V_{i}(g^{k}_i) = \frac{h\sigma_i}{2(\| \text{Ad}_{(g^{k}_i)^{-1}}e_{1}\|^{2}-6)}, \nonumber \end{equation*} |
where {g^{k}_i = \begin{bmatrix} \cos\theta^{k}_i & -\sin\theta^{k}_i & \phantom{-}x^{k}_i\\ \sin\theta^{k}_i & \phantom{-}\cos\theta^{k}_i & \phantom{-}y^{k}_i\\ 0 & \phantom{-}0 & \phantom{-}1 \end{bmatrix}\in SE(2)\nonumber} and {u^{k}_i = \sum\limits_{j = 1}^{3}u^{k}_{ij}e_{j}\in\mathfrak{se}(2)} . Also, in the discrete-time setting, the extended potential function V_{d, \text{ext}}: SE(2)\times \mathfrak{se}(2)\to\mathbb{R} can be constructed in exactly the same way as in the above example, and is given by
\begin{align} V_{i}^{0, ext}(g_{i}^{k}, \alpha_i) = \frac{h\sigma_i}{2(\| \text{Ad}_{(g^{k}_i)^{-1}}\alpha_i\|^{2}-6)}, \end{align} |
where {\alpha_{i}^{k} = \sum\limits_{j = 1}^{3} \alpha_{ij}^{k}e^{j}} . We do not give all the details again and leave it up to reader to verify that the assumptions (ⅰ) - (ⅶ) from (3.3) are satisfied. The discrete-time equations are
\begin{align} (d\mathcal{R}_{hu^{k}_i}^{-1})^{*}\mu^{k}_i & = (d\mathcal{R}_{-hu^{k-1}_i}^{-1})^{*}\mu^{k-1}_i+ \text{ad}_{\bar{\alpha}^{k}_i}^{*}\frac{\partial V_{i}^{0, ext}}{\partial\bar{\alpha}^{k}_i}\\ & = \Theta_1^i\sum\limits_{j\in\mathcal{N}_i}T^*_{\overline{e}}L_{g_i}\bigg(\frac{\partial V_{ij}}{\partial \theta_i}e^1+\frac{\partial V_{ij}}{\partial x_i}e^2+\frac{\partial V_{ij}}{\partial y_i}e^3\bigg), \\ \bar{\alpha}^{k+1}_i & = \text{Ad}_{\mathcal{R}(hu^{k}_i)^{-1}}\bar{\alpha}^{k}_i, \quad \bar{\alpha}^{0}_i = \text{Ad}_{(g^{0}_i)^{-1}}\alpha^{0}_i, \end{align} |
where
\begin{align} \text{ad}_{\bar{\alpha}^{k}_i}^{*}\frac{\partial V_{i}^{0, ext}}{\partial\bar{\alpha}^{k}_i} = \frac{h\sigma_i\bar{\alpha}^{k}_{i1}}{(\|\bar{\alpha}^k_i\|^{2}-6)^{2}}\begin{bmatrix} \phantom{-}0 & \phantom{-}0 & \phantom{-}0\\ \phantom{-}0 & \phantom{-}0 & \phantom{-}0\\ -\bar{\alpha}^{k}_{i3} & \phantom{-}\bar{\alpha}^{k}_{i2} & \phantom{-}0 \end{bmatrix}. \end{align} |
For numerical purposes, we first choose a suitable retraction map, like the Cayley map or the exponential map, and then compute the quantities (d\mathcal{R}_{hu^{k}_i}^{-1})^{*}\mu^{k}_i and (d\mathcal{R}_{-hu^{k-1}_i}^{-1})^{*}\mu^{k-1}_i . As an example, if we choose the Cayley map \text{cay}: \mathfrak{se}(2)\to\mathrm{SE}(2) as the retraction map, (see [38] and [17] for instance) then we have
\begin{align} [d\text{cay}_{hu_{i}^{k}}^{-1}]^{*}\mu_{i}^{k} = \begin{bmatrix} 0 & \frac{1}{2}\gamma_i & 0\\ -\frac{1}{2}\gamma_i & \phantom{-}0 & \phantom{-}0\\ \mu_{i2}^{k}-\frac{h u_{i1}^{k} \mu_{i3}^{k}}{2} & \frac{h u_{i1}^{k} \mu_{i2}^{k}}{2}+\mu_{i3}^{k} & \phantom{-}0 \end{bmatrix} \end{align} |
where
\begin{equation*} \gamma_i = \left(\frac{h^2 (u_{i1}^{k})^{2}}{4}+1\right)\mu_{i1}^{k}+\left(\frac{h^2 u_{i1}^{k} u_{i2}^{k}}{4}-\frac{h u_{i3}^{k}}{2}\right)\mu_{i2}^{k}+\left(\frac{h^2 u_{i1}^{k} u_{i3}^{k}}{4}+\frac{h u_{i2}^{k}}{2}\right)\mu_{i3}^{k} \end{equation*} |
and {\mu_{i}^{k} = \sum\limits_{j = 1}^{3} \mu_{ij}^{k}e^{j}} . Note that for {v = \sum\limits_{i = 1}^{3}v^{i}e_{i}\in\mathfrak{g}} , the matrix representation for d\text{cay}_{v}^{-1} is given by
\begin{align} [d\text{cay}_{v}^{-1}] = \begin{bmatrix} 1+\dfrac{(v^1)^2}{4} & \phantom{-}0 & \phantom{-}0\\ \dfrac{v^1v^2}{4}-\dfrac{v^3}{2} & \phantom{-}1 & \phantom{-}\dfrac{v^1}{2}\\ \dfrac{v^1v^3}{4}+\dfrac{v^2}{2} & -\dfrac{v^1}{2} & \phantom{-}1 \end{bmatrix}. \end{align} |
We studied the reduction by symmetry for optimality conditions of extremal in an OCP for collision and obstacle avoidance of left-invariant multi-agent control system on Lie groups, by exploiting the physical symmetries of the agents and obstacles. Reduced optimality conditions are obtained using techniques from variational calculus and Lagrangian mechanics on Lie groups, in the continuous-time and discrete-time settings. We applied the results to an OCP for multiple unmanned surface vehicles. The method proposed in this work allows the construction of position and velocity estimators, by discretizing the variational equation given in Theorem 4.1 - instead of discretizing the equations of motion - and by deriving variational integrators - see Theorem 5.2. The reduction of sufficient conditions for optimality will be also studied by using the notion of conjugate points, as in [42], in future work, as well as the reduction by symmetry of the variational obstacle avoidance problems [43] on semidirect products of Lie groups endowed with a bi-invariant metric on a Riemannian manifold.
L. Colombo is very grateful to A. Bloch, R. Gupta and T. Ohsawa for many useful comments and stimulating discussions during the last years on the topic of this paper, which is inspired by our common previous work.
The authors acknowledge financial support from the Spanish Ministry of Science and Innovation, under grants PID2019-106715GB-C21, MTM2016-76702-P.
We the authors declare that there are no conflicts of interest.
[1] |
Whitmeyer SJ, Karlstrom KE (2007) Tectonic model for the Proterozoic growth of North America. Geosphere 3: 220–259. doi: 10.1130/GES00055.1
![]() |
[2] |
Yuan H, Romanowicz B (2010) Lithospheric layering in the North American craton. Nature 466: 1063–1068. doi: 10.1038/nature09332
![]() |
[3] | Karlstrom KE, Amato JM, Williams ML, et al. (2004) Proterozoic Tectonic Evolution of the New Mexico Region: A Synthesis, In: Mack GH, Giles CA, The Geology of New Mexico, A Geologic History, 1 Ed., NMGS, 389–406. |
[4] | Davison DD, (1980) Precambrian geology of the Van Horn area, Texas, In: Dickerson PW, Hoffer JM, Callender JF, Trans Pecos Region (West Texas), New Mexico Geological Society 31st Annual Fall Field Conference Guidebook, 308: 151–154. |
[5] | Mosher S, Hoh AM, Zumbro JA, et al. (2004) Tectonic evolution of the eastern Llano uplift, central Texas: A record of Grenville orogenesis along the southern Laurentian margin. Mem Geol Sic Am 197: 783–798. |
[6] | Pulliam J, Grand SP, Xia Y, et al. (2010) Edge-Driven Convection Beneath the Rio Grande Rift, InSights the EarthScope. Summer 2010: 2. |
[7] | Gök R, Ni JF, West M, et al. (2003) Shear wave splitting and mantle flow beneath LA RISTRA. Geophys Res Lett 30: 16. |
[8] | West M, Ni J, Baldridge WS, et al. (2004) Crust and upper mantle shear wave structure of the Southwest United States: Implications for rifting and support for high elevation. J Geophys Res 109: 1–16. |
[9] |
Shen W, Ritzwoller MH, Schulte-Pelkum V (2013) A 3-D model of the crust and uppermost mantle beneath the Central and Western US by joint inversion of receiver functions and surface wave dispersion. J Geophys Res Solid Earth 118: 262–276. doi: 10.1029/2012JB009602
![]() |
[10] | Bryan S (2007) Silicic large igneous provinces. Episodes 30: 20–31. |
[11] |
Goodell PC, Mahar MA, Mickus KL, et al. (2017) The presence of a stable "Megablock" in the southwestern North American Proterozoic craton in northern Mexico. Precambrian Res 300: 273–288. doi: 10.1016/j.precamres.2017.08.008
![]() |
[12] | Warner LA, Holser WT, Wilmarth VR, et al. (1959) Occurrence of nonpegmatite beryllium in the United States. Geol Surv Prof Pap 318. |
[13] |
Aguirredíaz GJ, Labarthehernández G (2003) Fissure ignimbrites: Fissure-source origin for voluminous ignimbrites of the Sierra Madre Occidental and its relationship with Basin and Range faulting. Geology 31: 773–776. doi: 10.1130/G19665.1
![]() |
[14] | Ferrari L, López-Martínez M, Rosas-Elguera J (2002) Ignimbrite flare-up and deformation in the southern Sierra Madre Occidental, western Mexico: Implications for the late subduction history of the Farallon Plate. Tectonics 21: 1–23. |
[15] |
James EW, Henry CD (1991) Compositional changes in Trans-Pecos Texas Magmatism Coincident with Cenozoic stress realignment. J Geophys Res Solid Earth 96: 13561–13575. doi: 10.1029/91JB00203
![]() |
[16] | McLemore VT, North RM, Leppert S, (1988) Rare-earth elements (REE), niobium and thorium districts and occurrences. In: New Mexico: New Mexico Bureau of Mines and Mineral Resources, Open-file Report OF-324, 1–28. |
[17] |
Dunbar NW, Campbell AR, Candela PA (1996) Physical, chemical, and mineralogical evidence for magmatic fluid migration within the Capitan pluton, southeastern New Mexico. Geol Soc Am Bull 108: 318–333. doi: 10.1130/0016-7606(1996)108<0318:PCAMEF>2.3.CO;2
![]() |
[18] |
Gibson SA, Thompson RN, Leat PT, et al. (1993) Ultrapotassic Magmas along the Flanks of the Oligo-Miocene Rio Grande Rift, USA: Monitors of the zone of lithospheric mantle extensional and thinning beneath a continental rift. J Petrol 34: 187–228. doi: 10.1093/petrology/34.1.187
![]() |
[19] | TA, Transportable Seismic Network: Imaging the Earth's Interior. Available from: http://www.usarray.org/researchers/obs/transportable. |
[20] | SIEDCAR, Seismic Investigation of Edge Driven Convection Associated with The Rio Grande Rift. Flex Array. Available from: http://www.usarray.org/researchers/obs/flexible/deployments/siedcar/. |
[21] | Meltzer A, Rudnick R, Zeitler P, et al. (1999) The USArray Initiative. Geol Soc Am Today 9: 8–10. |
[22] | IRIS, Incorporated Research Institutions for Seismology. Available from: http://ds.iris.edu/ds/. |
[23] | Wilber 3, IRIS Wilber 3: Select Event. Available from: http://ds.iris.edu/wilber3/find_event. |
[24] | Ammon CJ (2001) Notes on Seismic Surface-Wave Processing. Part I. Group Velocity Estimation, Saint Louis University. Ver 3.9.0. Surface Wave Multiple Filter Analysis Software Documents. Available from: http://eqseis.geosc.psu.edu/~cammon/. |
[25] |
Singh AP, Mishra OP, Rastogi BK (2012) A new insight into crustal heterogeneity beneath the 2001 Bhuj earthquake region of northwest India and its implications for rupture initiations. J Asian Earth Sci 48: 31–42. doi: 10.1016/j.jseaes.2011.12.020
![]() |
[26] |
Dixit M, Singh AP, Mishra OP (2017) Rayleigh wave group velocity tomography of Gujarat region, Western India and its implications to mantle dynamics. J Seismol 21: 1–15. doi: 10.1007/s10950-016-9620-6
![]() |
[27] | USGS, United States Geological Survey, Mineral Resources On-Line Spatial Data. Available from: https://mrdata.usgs.gov/gravity/isostatic/. |
[28] |
Paterson NR, Reeves CV (1985) Applications of gravity and magnetic surveys; the state-of-the-art. Geophysics 50: 2558–2594. doi: 10.1190/1.1441884
![]() |
[29] | Phillips JD, Duval JS, Ambroziak RA (1993) National geophysical data grids; gamma-ray, gravity, magnetic, and topographic data for the conterminous United States. Data. |
[30] |
Wessel P, Smith WHF (1996) A global, self-consistent, hierarchical, high-resolution shoreline database. J Geophys Res Solid Earth 101: 8741–8743. doi: 10.1029/96JB00104
![]() |
[31] | Wessel P, Smith WHF, Scharroo R, et al. (2013) Generic Mapping Tools: Improved version released. EOS Trans Am Geophys Union 94: 409–410. |
[32] | NOAA's NCEI, National Oceanic and Atmospheric Administration's National Centers for Environmental Information. Available from: https://www.ngdc.noaa.gov/mgg/global/. |
[33] | Dean EA, Keller GR (1991) Interactive processing to obtain interstation surface-wave dispersion. AAPS J 4451: 1–6. |
[34] |
Kovach RL (1978) Seismic surface waves and crustal and upper mantle structure. Rev Geophys 16: 1–13. doi: 10.1029/RG016i001p00001
![]() |
[35] | Dziewonski A, Bloch S, Landisman M (1969) A technique for the analysis of transient seismic signals. Bull Seism Soc Am 59: 427–444. |
[36] | Herrin E, Goforth T (1977) Phase-matched filters: Application to the study of Rayleigh waves. Bull Seism Soc Am 67: 1259–1275. |
[37] | SAC, Seismic Analysis Code. Available from: https://ds.iris.edu/ds/nodes/dmc/software/downloads/sac/. |
[38] | Helffrich G, Wookey J, Bastow I (2013) The Seismic Analysis Code. In: A Primer and User's Guide, 1 Ed., Cambridge University Press. |
[39] | Butterworth S (1929) On the theory of filter amplifiers. Wireless Engineer. |
[40] | Bianchi G (2007) Electronic filter simulation & design. Amacom. |
[41] |
Kennett BLN, Engdahl ER, Buland R (1995) Constraints on seismic velocities in the Earth from travel times. Geophys J Int 122: 108–124. doi: 10.1111/j.1365-246X.1995.tb03540.x
![]() |
[42] |
Ward KM (2015) Ambient noise tomography across the southern Alaskan Cordillera. Geophys Res Lett 42: 3218–3227. doi: 10.1002/2015GL063613
![]() |
[43] | EARS, The EarthScope Automated Receiver Survey. Available from: http://ears.iris.washington.edu/. |
[44] | Ewing TE (1991) The Tectonic framework of Texas: Text to accompany "The Tectonic map of Texas", In: Bureau of Economic Geology, Publication SM0001: 1–36. |
[45] | GNU Octave. Scientific Programing Language. Available from: https://www.gnu.org/software/octave/. |
[46] | Hansen JS (2011) GNU Octave Beginner's Guide. Packt Publishing Ltd. |
[47] | Hanselman D, Littefield B (1998) Mastering Matlab 5. Prentice Hall Inc. |
[48] | MathWorks Documentation. Available from: https://www.mathworks.com/help/. |
[49] | Holzbecher E, Si H (2008) Accuracy Test for COMSOL -and Delaunay Meshes. Excerpt from the Proceedings of the COMSOL Conference 2008 Hanover. |
[50] |
Jänicke L, Kost A (1996) Error estimation and adaptive mesh generation in the 2D and 3D finite element method. IEEE Trans Magn 32: 1334–1337. doi: 10.1109/20.497492
![]() |
[51] | Stein S, Wysession M, (2002) An introduction to seismology, In: Earthquakes and Earth Structure, 1 Ed., Blackwell Pub. |