Variational point-obstacle avoidance on Riemannian manifolds
Anthony Bloch Margarida Camarinha Leonardo Colombo





[mytitlenote]The research of A. Bloch was supported by NSF grants DMS-1207693, DMS-1613819 and ASFOR. The research of M. Camarinha was partially supported by the Centre for Mathematics of the University of Coimbra – UID/MAT/00324/2019, funded by the Portuguese Government through FCT/MEC and co-funded by the European Regional
Development Fund through the Partnership Agreement PT2020. L. Colombo was supported by This work was partially supported by I-Link Project (Ref: linkA20079) from CSIC; Ministerio de
Economia, Industria y Competitividad (MINEICO, Spain) under grant MTM2016-76702-P; `Severo
Ochoa Programme for Centres of Excellence" in R&D (SEV-2015-0554).The project that gave rise to these results received the support of a fellowship from “la Caixa' Foundation (ID 100010434). The
fellowship code is LCF/BQ/PI19/11690016.



Department of Mathematics, University of Michigan
530 Church St. Ann Arbor, 48109, Michigan, USA
[1]Email adress: abloch@umich.edu.


Centre for Mathematics of the University of Coimbra, Department of Mathematics
University of Coimbra, 3001-501 Coimbra, Portugal

[2]Email adress: mmlsc@mat.uc.pt.




Instituto de Ciencias Matemáticas, Consejo Superior de Investigaciones Científicas
Calle Nicolás Cabrera 13-15, Cantoblanco, 28049, Madrid, Spain
[3]Email adress: leo.colombo@icmat.es.








In this letter we study variational obstacle avoidance problems on complete Riemannian manifolds. The problem consists of minimizing an energy functional depending on the velocity, covariant acceleration and a repulsive potential function used to avoid a static obstacle on the manifold, among a set of admissible curves. We derive the dynamical equations for extrema of the variational problem, in particular on compact connected Lie groups and Riemannian symmetric spaces. Numerical examples are presented to illustrate the proposed method.


Geometric methodsRiemannian geometryPath planningSymmetric spacesObstacle avoidance.
[2010] Primary: 37K05 Secondary: 37J1537N0534A3834C1434C25







§ INTRODUCTION

Many problems in physics, engineering and related disciplines can be formulated as variational problems. Sometimes the solution we seek has to satisfy some nonlinear constraints or avoid static or moving obstacles in the space of configurations of a given system. Riemannian manifolds are, in many cases, the suitable configuration spaces to model these problems. Variational problems on Riemannian manifolds have been extensively studied in the last decades for applications ranging from trajectory planning in aerospace engineering <cit.>, <cit.>, interpolation of data in medical images and pattern recognition <cit.> to parametric regression of data for computer vision problems <cit.>. A basic reference for  variational theory on Riemannian manifolds is the book of Milnor <cit.>. This key procedure, which is Lagrangian in nature, is to study the characterization  of critical paths of an action functional over a set of admissible curves.

Since then, a number of papers have been devoted to the generalization of this variational theory in many other contexts: interpolation problems <cit.>, collision avoidance of multiple agents <cit.> and quantum splines interpolation <cit.>, among others. There are various treatments of obstacle avoidance in different contexts, nevertheless, to the best of our knowledge, the point-obstacle avoidance  problem, that is, the problem of creating feasible, safe paths that avoid a prescribed point-obstacle while minimizing some quantity such as energy or time in the Riemannian manifold setting has not been widely discussed in the literature.


We studied trajectory planning schemes with obstacle avoidance on Riemannian manifolds from the variational point of view in our previous work <cit.>. Inspired by the goal of gaining a better understanding of trajectories which minimize a weighted combination of the covariant acceleration and  the velocity of the system in the presence of a repulsive potential which is  used to avoid a static circular obstacle, in <cit.> we extended the problem to the trajectories that also interpolate some points on the manifold. The present work goes one step further and considers variational obstacle avoidance problems on complete Riemannian manifolds with the obstacle being a specified configuration represented by an element in the manifold. The aim is to study necessary optimality conditions for the problem for different systems on Riemannian  Lie groups and symmetric spaces.



Specifically, the problem studied in this work consists of finding  variational trajectories surrounding a given obstacle, among a set of admissible curves, which minimize an energy
functional that depends on the velocity and covariant acceleration. An artificial potential function is used to prevent the trajectory to cross a given point-obstacle. To solve the problem, we employ techniques from the calculus of variations on Riemannian manifolds, taking into account that the problem under study can be seen as a higher order variational problem <cit.>, <cit.>, <cit.>, <cit.>, <cit.>, <cit.>.

The main contribution is to provide necessary optimality conditions for the obstacle avoidance problem  on a Riemannian manifold (i.e., a nonlinear space), based on differential equations on a vector space (i.e., a linear space).

This procedure is possible due to the bi-invariance of the Riemannian metric on the Lie group, which allows us to left translate the higher-order covariant derivatives of the trajectory to the Lie algebra.  The potential function is expressed in terms of the exponential map, defined by the geodesic connecting the  configuration of the system with  the obstacle configuration, and its gradient can also be left translated to the identity.  One advantage of this method consists in the fact that, if we are dealing with the problem in a symmetric space, then we can lift the trajectories to the Lie group acting on it, solve the equations there, and project back the trajectories to the symmetric space. The assumption of having complete manifolds is essential to guarantee that every geodesic in the symmetric space is the projection of a horizontal geodesic in the Lie group, thus allowing one to study the problem in the Lie group. From this point of view, the problem studied in this work extends recent developments for cubics in tension on symmetric spaces presented in <cit.>, <cit.>.


The structure of the paper is as follows. We start by introducing the geometric framework  on a Riemannian manifold that is necessary to study the variational problem in Sec. II. Next, in Sec. III, we introduce variational point-obstacle avoidance problems on complete Riemannian manifolds and we derive necessary optimality conditions. A special emphasis is given to the case of compact and connected Lie groups with the illustrative example of the rigid body in SO(3). Finally in Section V we analyse the problem on Riemannian symmetric spaces lifting the equations to the Lie group acting on the manifold. Numerical examples on the sphere S^2 are presented to illustrate the proposed method. 




§ PRELIMINARIES ON RIEMANNIAN GEOMETRY 

Let M be an n-dimensional  Riemannian
manifold with Riemannian metric denoted by
⟨·,·⟩:T_xM× T_xM→ℝ at each point x∈ M, where T_xM is
the tangent space of M at x. The length of a tangent vector is determined by its norm,
||v_x||=⟨ v_x,v_x⟩^1/2 with v_x∈ T_xM. A Riemannian connection ∇ on M is a map that assigns to any two smooth vector fields X and Y on M a new vector field, ∇_XY. For the properties of ∇, we refer the reader to <cit.>.  The operator
∇_X, which assigns to every vector field Y the vector
field ∇_XY, is called the covariant derivative of
Y with respect to X.

Given vector fields X, Y
and Z on M, the vector field R(X,Y)Z given by 
    R(X,Y)Z=∇_X∇_YZ-∇_Y∇_XZ-∇_[X,Y]Z
  is called the curvature tensor of M. R is trilinear in X, Y and Z, and a tensor of type (1,3).


Consider a vector field W  along a curve x on M. The sth-order covariant  derivative along x  of W is denoted by D^sW/dt^s, s≥ 1. We also denote by D^s+1x/dt^s+1 the sth-order covariant derivative along x of the velocity vector field of x, s≥ 1.


A vector field X along a piecewise smooth curve x in M is said to be parallel along x if DX/dt≡ 0. If x_0=x(0) is the initial point of the curve x and Y∈ T_x_0M is an arbitrary tangent vector to M at x_0, then there exists a unique parallel vector field X along x having the value  Y at x_0. When the velocity vector field of a curve x is parallel, the curve x is called a geodesic.

We assume here that M is complete, which implies that any two points p and q in M can be connected by a shortest arc  γ_p,q. In such a context the Riemannian distance between two points in M, d:M× M→ℝ can be defined by

    d^2(p,q)=∫_0^1d γ_p,q/d s(s)^2  ds.

Additionally, if we assume that the points p and q belong to a convex open  ball ℬ, the Riemannian exponential map _q is a diffeomorphism in ℬ and we can  write the Riemannian distance by means of the Riemannian exponential on M as

    d(q,p)=_q^-1p.























§ VARIATIONAL OBSTACLE AVOIDANCE PROBLEM ON COMPLETE RIEMANNIAN MANIFOLDS




 §.§ Problem formulation and dynamical equations




Let M be a complete Riemannian manifold, T, σ and τ  be positive real numbers, (p_0,v_0), (p_T,v_T) points in TM  and S a regular submanifold of M. Consider the set Ω of admissible curves, all  𝒞^1 piecewise smooth curves  on M, x:[0,T]→ M, verifying the boundary conditions 
    x(0)=p_0,     x(T)=p_T,  dx/dt(0)=v_0,   dx/dt(T)=v_T,
 and define the functional J on Ω given by

    J(x)=∫_0^T1/2(D^2x/dt^2(t)^2+
     σ dx/dt(t)^2+V(x(t)))dt.


The functional (<ref>) is  given by a weighted combination of the velocity and  covariant acceleration of the curve x regulated by the
parameter σ, together with an artificial potential function V:M→ℝ used to ensure collision avoidance with an static obstacle which is given by a configuration q on M.


 The function V is assumed to be at least C^2 and associated with a fictitious force inducing a repulsion from q, defined as the inverse value of a distance function specified by the Riemannian exponential. Then, V  goes to infinity  near q and decays to zero at some positive level set far away from the obstacle q. This ensures that  trajectories given by solutions of the variational problem do not intersect q.  The use of artificial potential functions to avoid collision was introduced in Khatib (see <cit.> and references therein) and further studied for example by Koditschek and Rimon <cit.>, <cit.>.




For the class of admissible curves x, we introduce the 𝒞^1-piecewise smooth one-parameter admissible variation of  x as α : (-ϵ , ϵ ) × [0,T]  →  M ;(r,t) ↦α (r,t)=α_r(t)
verifying α_0(t)=x(t) and α_r∈Ω, for each r∈ (-ϵ , ϵ ).
The variational vector field  associated to a one-parameter admissible variation α is a 𝒞^1-piecewise smooth vector field along x defined by 
    X(t)=D/∂ r|_r=0α(r,t)∈ T_xΩ,

verifying  the boundary conditions

    X(0)=0,     X(T)=0,  DX/dt(0)=0,  DX/dt(T)=0.


The admissible set  Ω admits an infinite dimensional Hilbert manifold structure (see <cit.> and references therein) and its tangent space  T_xΩ at x can be identified with the
 vector space   of all 𝒞^1 piecewise smooth vector
fields X along x  verifying the boundary conditions (<ref>).




Problem: The variational obstacle avoidance problem consists of minimizing the functional J among Ω satisfying the boundary condition (<ref>).

In <cit.> we proved the following result as a solution of the previous problem:

 
A necessary condition for x  to be a minimizer of the functional (<ref>) over the class Ω satisfying the boundary condition (<ref>) is that, x is smooth on [0,T], and verifies the following equation
 
    D^4x/dt^4+R(D^2x/dt^2,dx/dt)dx/dt- σD^2x/dt^2+ 1/2V(x) ≡ 0.



We consider the repulsive potential function defined by V(x)=τ/d^2(q,x), x∈ M. Such a potential function gives tractable formulas for the gradient of V in terms of the exponential map as we will see in Lemma <ref>(see <cit.> for more details in the subject).

In this paper we assume that the points x_0,x_T∈ M are sufficiently close to guarantee the exponential map is global diffeomorphism, which means that we restrict our analysis to an convex open neighborhood of the obstacle containing x_0 and x_T.



If B_q is a convex open ball containing q and V is the function defined  by V(p)=τ/d^2(q,p) in B_q, then its gradient can be written in the form 
    grad V(p)=τ/d^4(q,p)exp_p^-1q.



Proof:

If we consider a map α: r→α(r) verifying α(0)=x  and  the family of geodesics from q to α(r) is given by
γ(s,r)=_q(s _q^-1α(r)), then we have

    d/d rd^2(q,α)=⟨∂γ/∂ r ,∂γ/∂
    s|_s=1⟩=-⟨d α/dr ,_α^-1q⟩.

Therefore, the gradient vector field of V is given by
V(x)=τ/d^4(q,p)exp_p^-1q. □

As a consequence of Theorem <ref> and Lemma <ref> we have the following result for the obstacle avoidance problem on a complete Riemannian manifold.

 A necessary condition for x  to be a minimizer of the functional (<ref>) over the class Ω satisfying the boundary condition (<ref>) is that, x∈ M is smooth on [0,T], and verifies the following equations

    D^4x/dt^4+R(D^2x/dt^2,dx/dt)dx/dt- σD^2x/dt^2+1/2τ/d^4(q,x)exp_x^-1q ≡ 0.







 §.§ Variational obstacle avoidance problem on compact and connected Lie groups


Let G be a compact and connected Lie group endowed with a bi-invariant Riemannian metric ⟨· , ·⟩ and 𝔤 its Lie algebra. The following result from <cit.> provides a formula for the covariant derivative ∇ and the curvature tensor R in terms of the Lie algebra structure.



  (i) Every compact and connected Lie group admits a left and right invariant metric ⟨·,·⟩.

  (ii) If ∇ denotes the corresponding Levi-Civita  connection induced by the metric ⟨·,·⟩ and X,Y and Z are left-invariant vector fields on G then 
    ∇_XY=1/2[X,Y]  R(X,Y)Z=-1/4[[X,Y],Z].




This lemma guarantees that the connection ∇ is completely determined by its restriction to 𝔤 via left-translations. This restriction, denoted by 𝔤∇:𝔤×𝔤→𝔤,  is naturally given by 𝔤∇_wu= 1/2 [w,u] (see <cit.> p. 271).
Indeed, if u,w∈𝔤 we have ∇_w_Lu_L=(𝔤∇_wu)_L, where u_L denotes the left-invariant vector field  associated  to u.



Let x:I⊂ℝ→ G be a smooth curve on G. The body velocity of x is the curve v:I⊂ℝ→𝔤 defined by v(t)=T_x(t)L_x(t)^-1(dx/dt(t)), where L_g:G→ G denotes the left-translation map by g.

Let {e_1,…,e_n} be a basis of  𝔤. Consider the body velocity of x on the given basis, defined by v=∑_i=1^n v_i e_i.
To write the equations determining necessary conditions for extremal, we use the following formulas,  where 𝔤∇_wv=∑_i,j=1^n w_i v_j∇_e_je_i and e denotes the identity element on G (see for instance <cit.>).

    dx/dt=T_eL_xv,  D^2x/dt^2=T_eL_x(v^'+𝔤∇_vv),
       D^3x/dt^3=T_eL_x(v^''+𝔤∇_v^'v+2 𝔤∇_vv^'+𝔤∇_v𝔤∇_vv),
       D^4x/dt^4=T_eL_x(v”'+𝔤∇_v”v+3𝔤∇_v^'v^'+3 𝔤∇_vv”+𝔤∇_v^'𝔤∇_vv+2 𝔤∇_v𝔤∇_v^'v+3𝔤∇_v^2v^'+𝔤∇^3_vv),
       R(D^2x/dt^2,dx/dt)dx/dt=T_eL_x(ℜ(v^',v)v+ℜ(𝔤∇_vv,v)v),
 where ℜ denotes the curvature tensor associated with 𝔤∇.

Using Theorem <ref>, the previous formulas are reduced to 
    D^2x/dt^2=T_eL_xv^',
       D^3x/dt^3=T_eL_x(v^''+1/2[v, v^']),
       D^4x/dt^4=T_eL_x(v”'+[v,v”]+1/4[[v^',v],v]),
       R(D^2x/dt^2,dx/dt)dx/dt=-1/4T_eL_x([[v^',v],v]).




Let G be a compact and connected Lie group, then for all x,y∈ G the following identity holds 
    exp_y^-1(x)=T_eL_yexp_e^-1(y^-1x)



Proof: Let γ be a geodesic starting at γ(0)=e and finishing at γ(1)=y^-1x with γ'(0)=exp_e^-1(y^-1x). The curve β(t)=L_y∘γ(t) is a curve such that β(0)=y and β(1)=x with β'(0)=T_eL_yexp_e^-1(y^-1x).

Given that ∇ is left-invariant, β is a geodesic, and therefore T_eL_yexp_e^-1(y^-1x)=exp_y^-1(x).□




Using (<ref>)-(<ref>) and Lemma (<ref>) the equations (<ref>) for  the variational obstacle avoidance problem on a connected and compact Lie group are given as follows.


Let x be a curve on a connected and compact Lie group G with body velocity v with respect to the basis {e_1,…,e_n} in 𝔤. A necessary condition for x  to be a minimizer of the functional (<ref>) over the class of curves Ω satisfying the boundary conditions (<ref>) is that, x is smooth on [0,T], and the curve v in 𝔤 verifies the following equation
 
    v”'-σ v'+[v,v”]+τ/2||exp_e^-1(x^-1q)||^4exp_e^-1(x^-1q) ≡ 0.





This example is motivated by the fact that obstacle avoidance problems defined on the special orthogonal group SO(3) are often used to model avoidance of certain orientations of the rigid body. This is for instance the case for planning motion of an optical instrument where avoiding pointing at a certain light source is crucial.

We consider the variational obstacle avoidance problem on the Lie group SO(3). The Lie algebra 𝔰𝔬(3) is given by the set of 3× 3 skew-symmetric matrices.


Denote by t→ R(t) a curve on SO(3). The columns of the
matrix R(t) represent the directions of the principal axis of the
 body reference system at time t with respect to some fixed reference system.

 It is well known that 𝔰𝔬(3)≃^3, where the Lie bracket of matrices is identified with the cross product. This Lie algebra isomorphism  is the hat map ·̂:ℝ^3→𝔰𝔬(3) that assigns a matrix A∈𝔰𝔬(3), that is, a skew-symmetric matrix of the form A=(
               [    0 -a_3  a_2;  a_3    0 -a_1; -a_2  a_1    0;      ])  to the vector a=(a_1,a_2,a_3)∈ℝ^3. The matrix A can be denoted by  a. We endow SO(3) with the bi-invariant metric  ⟨· , ·⟩  corresponding to the usual inner product in ^3 via the hat isomorphism.
By Lemma <ref>, the Levi-Civita connection ∇ induced by ⟨· , ·⟩ is completely determined by its restriction to the Lie algebra 𝔰𝔬(3) given by
𝔰𝔬(3)∇_v z=1/2v× z
and the  restriction of the curvature tensor  to  𝔰𝔬(3) is defined by
ℜ(v,z)w=-1/4 (v× z)× w
where v,z,w ∈ℝ^3.





 









For the obstacle avoidance problem we consider the artificial potential V:SO(3)→ℝ given by 
    V(R)=τ/^-1(R^TQ)^2
 with τ∈ℝ^+ and Q,R∈ SO(3), with exp:𝔰𝔬(3)→ SO(3) denoting the matrix exponential map on SO(3) given by expξ=e^ξ with ξ∈𝔰𝔬(3).  The matrix exponential map is a diffeomorphism between U={a∈𝔰𝔬(3): a<π, a∈^3} and  V={R∈ SO(3): Tr≠ -1} and  its inverse map is  the matrix logarithm map.
Using the matrix logarithm, 
    exp^-1(R^TQ)/exp^-1(R^TQ)^4=log(R^TQ)/||log(R^TQ)||^4.
 Denoting ϕ=arccos(1/2(Tr(R^TQ)-1), and using Proposition 5.7 in <cit.>,  for R≠ Q[If R=Q, log(R^TQ)=0 and this case is outside the problem formulations since we are in the obstacle], 
    log(R^TQ)=ϕ/2sin(ϕ)(R^TQ-R^TQ).
  Since log(R^TQ)=ϕ it follows that


    log(R^TQ)/||log(R^TQ)||^4=1/2ϕ^3sin(ϕ)(R^TQ-RQ^T).



The body velocity of the curve R in SO(3) is the curve v  in 𝔰𝔬(3) verifying R'=Rv. Therefore, by  Proposition <ref> the minimizers for the obstacle avoidance problem on SO(3) verify the equation


    v”'= v”× v + σ v'+τ/4ϕ^3sin(ϕ)(RQ^T-QR^T)^×.

together with the equation R'=Rv, σ∈ℝ, τ∈ℝ^+ and the boundary conditions R(0)=R_0, R(T)=R_N, v(0)=v_0, v(T)=v_N, where ×:𝔰𝔬(3)→ℝ^3 denotes the inverse of the hat isomorphism ·̂.

Note that in the absence of obstacles, the extremals  reduce to the cubic polynomials in tension on SO(3) (see <cit.>) which equations are given by solutions of the equation v”'= v”× v + σ v'.




§ APPLICATION TO VARIATIONAL OBSTACLE AVOIDANCE PROBLEM ON RIEMANNIAN SYMMETRIC SPACES

Let H:=G/K be a Riemannian symmetric space, where G is a connected  finite-dimensional Lie group endowed with a bi-invariant Riemannian metric
and   K a closed Lie subgroup of G.

It is well known that the canonical projection π:G→ H is a Riemannian submersion (see <cit.> for instance), in the sense that, for all g in G, the isomorphism T_gπ:( T_gπ)^→ T_π(g)H preserves the inner-products defined by the Riemannian metrics on G and H and T_gG splits naturally into two orthogonal subspaces, the vertical subspace V_g:= T_gπ and the horizontal subspace Hor_g=(V_g)^⊥:=( T_gπ)^. The corresponding projections of T_gG onto V_g and Hor_g are denoted by 𝒱 and ℋ.

In particular,  the Lie algebra 𝔤 of G admits the decomposition
𝔤=𝔰⊕𝔪 where 𝔰 is the Lie algebra of K and 𝔪≃ T_𝔬H, whith  0=π(e), being e the identity element on G. That is, T_eπ=𝔰  and the horizontal subspace ( T_gπ)^ is 𝔪. Moreover, the following relations hold (see <cit.>)

    [𝔰,𝔰]⊂𝔰,  [𝔪,𝔪]⊂𝔰,  [𝔪,𝔰]⊂𝔪.


Using the decomposition of T_gG and defining vertical and horizontal tangent vectors on G, it is possible to define horizontal  curves and vector fields on H  to G , by choosing horizontal tangent vectors. We consider the horizontal curve x̃ on G verifying equations v=T_x̃(t)L_x̃^-1(dx̃/dt) and

    v”'+[v, [v^',v]]- σ v^'+1/2τ/exp_e^-1(x̃^-1q̃)^4exp_e^-1(x̃^-1q̃)≡ 0,
 the latter equation being that on the subspace 𝔪 and with q̃∈π^-1(q).


According to <cit.> and <cit.>, if we project x̃ to H by π, we obtain a curve x on H verifying equations (<ref>), that is, if we are able to find a curve v, and the corresponding curve x̃, verifying (<ref>) and dx̃/dt=T_eL_x̃v, a solution  of  (<ref>) can by obtained by  projecting the curve x̃ to H.



Consider the symmetric space H=S^2, the two-dimensional unit sphere, where G=SO(3) and K=SO(2). Denoting the canonical basis of ^3 by {e_1,e_2,e_3}, the group SO(2) can be seen as the subgroup of SO(3) leaving e_1 ∈ S^2 fixed. 

Each element x of S^2 can be represented by an element R in SO(3) by the relation x=Re_1 and the projection π:SO(3)→ S^2 is given by π(R)=Re_1. Note that the Lie algebra decomposition
𝔰𝔬(3)=𝔰⊕𝔪 corresponds to ^3=𝔰⊕𝔪 via the hat isomorphism, with 𝔰=span{e_1}  and 𝔪=span{e_2,e_3}.





Let q be the obstacle in S^2. To obtain the extremals for the obstacle avoidance problem it is enough to solve  the following differential equations in SO(3)×𝔪


    v”'=(v^'× v)× v+σ v^'+τ/4ϕ^3sin(ϕ)(RQ^T-QR^T)^×
 and R'=Rv, with Q∈ SO(3) verifying the condition π(Q)=q. Next we project the solution R to S^2.


Simulation results: We now show some simulation results demonstrating applicability of the proposed method in the obstacle avoidance problem on the sphere. In all simulations we employ an Euler method with step size h=0.001. We consider R(0) to be  the (3× 3) identity matrix for all the simulations.


  (1) Obstacles along a geodesic: We first consider a situation with σ=0. In the absence of an obstacle, the solution of equation (<ref>) is a geodesic. We consider initial values v(0)=(0,0,1), v'(0)=v”(0)=(0,0,0) and display the result in Figure <ref>. We chose a random value q which is the obstacle along the geodesic, and next we found a representative for q in SO(3), denoted Q̃. Note that we have the freedom of choosing one angle in SO(3) since the sphere only provides two of the three Euler angles describing an element of SO(3) (i.e., we have infinitely many choices of such a value). We choose π/4. We solve the equations on SO(3) and then project back to the sphere S^2. In Figure <ref> we display, with τ=1 the solution on S^2.












































  (2)Obstacle along cubics in tension: Finally we show how the method works with cubics in tension. We consider the cubic in tension trajectory, with the point-obstacle along the curve and we want to design a trajectory that avoid the point-obstacle. We consider initial conditions v(0)=(0,4,-1),  v'(0)=(0,-0.3,0.5), v”(0)=(0,-1,2) and σ=1. If we set the parameter τ=1 we obtain practically the same trajectory. The trajectories are extremely close in value at all points, and in particular, the red curve is at a distance of 2.54×10^-6 from the obstacle placed in the blue curve as it is show in Figure <ref>.
  









 However, we can increase τ and smoothly deform  the trajectory from the obstacle as τ increases. We show the comparison between the obstacle avoidance and the cubic in tension with the obstacle along it as τ increases in Figure <ref>.














 xx
 ligiaL Abrunheiro, M Camarinha, J Clemente-Gallardo, JC Cuchí, P Santos.  A general framework for quantum splines International Journal of Geometric Methods in Modern Physics 15 (09), 1850147, 2018.

mishal
M. Assif, R. Banavar, A. Bloch, M. Camarinha, L Colombo.  Variational collision avoidance problems on Riemannian manifolds. in Proceedings of the IEEE International
Conference on Decision and Control, 2018, pp. 2791-2796.
  Bl A. Bloch, J. Baillieul, P. E. Crouch, J. E. Marsden, D. Zenkov, Nonholonomic Mechanics and Control.  New York, NY: Springer-Verlag, 2nd ed. 2015.

BlCaCoCDC A. Bloch, M. Camarinha, L. Colombo. Variational obstacle avoidance
on Riemannian manifolds. in Proceedings of the IEEE International
Conference on Decision and Control, 2017, pp. 146-150.
BlCaCoIJC A. Bloch, M. Camarinha and L. J. Colombo. Dynamic interpolation for obstacle avoidance on Riemannian manifolds. International Journal of Control, pages 1-22, doi:10.1080/00207179.2019.1603400, <https://doi.org/10.1080/00207179.2019.1603400>.
Preprint available at. arXiv:1809.03168 [math.OC]
BlCr A. Bloch and P. Crouch. On the equivalence of higher order variational
problems and optimal control problems. in Proceedings of the IEEE
International Conference on Decision and Control, Kobe, Japan, 1996,
pp. 1648-1653.
Boothby W.  M.  Boothby, An  Introduction  to  Differentiable  Manifolds  and Riemannian Geometry. Orlando,  FL: Academic  Press Inc., 1975.

bookBullo  F. Bullo and A. D. Lewis, Geometric Control of Mechanical Systems. Springer-Verlag,  2004.

MargaridaThesis M. Camarinha. The geometry of cubic polynomials in Riemannian manifolds.
Ph.D. thesis, Univ. de Coimbra, 1996.

CoFeMdD  L. Colombo, S. Ferraro, D. Martín de Diego. Geometric integrators for higher-order variational systems and their application to optimal control. J. Nonlinear Sci. 26 (2016), no. 6, 1615-1650.
CoMdM L. Colombo and D. Martín de Diego. Higher-order variational problems on Lie groups and optimal control applications. J. Geom. Mech.
6 (2014), no. 4, 451-478.
Cophd  L. Colombo. Geometric and numerical methods for optimal control of
mechanical systems. PhD thesis, Instituto de Ciencias Matemáticas,
ICMAT (CSICUAM-UCM-UC3M), 2014.

CroSil:95 P. Crouch and F. Silva Leite, The dynamic interpolation problem: on Riemannian manifolds, Lie groups, and symmetric spaces,  J. Dynam. Control Systems 1 (1995), no. 2, 177–202.

Giambo R. Giambó,
F. Giannoni, P. Piccione. “An analytical theory for Riemannian cubic polynomials". IMA J Math Control Inform 19:445-460, 2002.

Helgason S. Helgason, Differential geometry, Lie groups, and symmetric spaces, Pure and
Applied Mathematics, no. 80, Academic Press, Oxford, 1978.
Hinkle J. Hinkle, P.T. Fletcher, S. Joshi. Intrinsic polynomials for regression on Riemannian
manifolds. Journal of Mathematical Imaging and Vision 50 (1-2), 32-52, 2014.
Hong
Y. Hong, R. Kwitt, N. Singh, N. Vasconcelos, and M. Niethammer. Parametric Regression on the Grassmannian IEEE Transactions on Pattern Analysis and Machine Intelligence, 2016.

BlochHussein
I. Hussein  and A. Bloch.
Dynamic Coverage Optimal Control for Multiple Spacecraft Interferometric Imaging. Journal of Dynamical and Control Systems, Vol. 13, Issue 1, pp 69-93, 2007

Jackson J. Jackson, Dynamic interpolation and application to flight control. PhD Thesis.Arizona State Univ., 1990.
Karcher77H. Karcher. Riemannian center of mass and mollifier smoothing
Comm. Pure Appl. Math., 30: 509-541, 1977.
Khatib1986 O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots. Int. J. of Robotics Research, vol 5, n1, 90–98,1986.

K88
D. Koditschek. Robot planning and control via potential functions
Robotics Review. MIT Press, Cambridge, MA. 1992.
Koditschek1990
D. Koditschek and E. Rimon. Robot navigation functions on manifolds with boundary
Adv. in Appl. Math., Vol11, n4, 412–442, 1990.

Milnor J. Milnor, Morse Theory.  Princeton, NJ: Princeton Univ. Press, 2002.

Noa:89 L. Noakes, G. Heinzinger and B. Paden, Cubic Splines on
Curved Spaces, IMA Journal of Math. Control & Inf. 6,
(1989), 465–473.
Nomizu K. Nomizu, Invariant affine connections on homogeneous spaces. Amer. J. Math 76, pp. 33-65, 1954.
Oneill B. O' Neill. Submersions and geodesics. Duke Math. J. 34, 363-373, 29, 1967.
SCC00 F. Silva Leite, M. Camarinha and P. Crouch, Elastic curves as solutions of Riemannian and sub-Riemannian control problems Math. Control Signals Systems 13, no. 2, 140–155, (2000).
ZN E. Zhang, L. Noakes. Left Lie reduction for curves in homogeneous spaces. Adv Comput Math.44 (5), 1673-1686, 2018.
ZN1E Zhang, L Noakes. Relative geodesics in bi-invariant Lie groups
Proceedings of the Royal Society A: Mathematical, Physical and Engineering Sciences, 473, 20160619, 2017.



