On Sub-Riemannian Geodesics in SE(3) Whose Spatial Projections do not Have Cusps

Let \({\mathbf {x}}_{0}, {\mathbf {x}}_{1} \in \mathbb {R}^{3}\) and \({\mathbf {n}}_{0}, {\mathbf {n}}_{1} \in S^{2} = \{\mathbf {v} \in \mathbb {R}^{3} | \|\mathbf {v}\| = 1\}\). Our goal is to find an arc-length parameterized curve s↦x(s) such that

We assume that the boundary conditions (x ₀,n ₀) and (x ₁,n ₁) are chosen such that a minimizer exists. Due to rotation and translational invariance of the problem, it is equivalent to the problem with the same functional and boundary conditions (0,e _z ) and \((R_{\mathbf {n}_{0}}^{T}({\mathbf {x}}_{1}-{\mathbf {x}}_{0}), R_{\mathbf {n}_{0}}^{T}{\mathbf {n}}_{1})\), where e _z denotes the unit vector in the z-axis in the right handed {x,y,z} coordinate system and \(R_{\mathbf {n}_{0}}\in \text {SO}(3)\) such that \({\mathbf {n}}_{0} = R_{\mathbf {n}_{0}}{\mathbf {e}}_{z}\). Therefore, without loss of generality, we set (unless explicitly stated otherwise) x ₀ = 0 and n ₀ = e _z for the remainder of the article. Hence, the problem now is to find a sufficiently smooth arc-length parameterized curve s↦x(s) such that

We refer to the above problem as P _{c u r v e} .

In this paper, we use two different parameterizations: spatial arclength s and sub-Riemannian arclength \(t(s) = {{\int }_{0}^{s}} \sqrt {\xi ^{2} + \kappa ^{2}(\sigma )} \, \mathrm {d}\sigma \). We denote the derivative \(\frac {d}{d s}\) by a prime, and \(\frac {d}{d t}\) by a dot.

In Section 2, we lift problem P _{c u r v e} on \(\mathbb {R}^{3}\) to a sub-Riemannian problem P _{m e c} on the quotient where SO(2) is identified with all rotations in \(\mathbb {R}^{3}\) about reference axis e _z . Such an extension and naming (‘mec’ refers to mechanical) was also done for the problem P _{c u r v e} on \(\mathbb {R}^{2}\), cf. [6, 11].

To state the problem P _{m e c} on the quotient (1.2), we first resort to the corresponding left-invariant sub-Riemannian problem P _{M E C} on the Lie group SE(3). We formulate problem P _{M E C} in Definition 1 and problem P _{m e c} in Definition 3.

The main result in Section 2 is Theorem 1, where we show the two requirements for sub-Riemannian geodesics γ(⋅)=(x(⋅),R(⋅)) in P _{M E C} to have the property that the corresponding spatially projected curve x(⋅) is indeed a stationary curve of problem P _{c u r v e} . There, we also show that these sub-Riemannian geodesics in SE(3) relate to well-defined geodesics of problem P _{m e c} on the quotient \(\mathbb {R}^{3}\rtimes S^{2}\). One of the two requirements is a vanishing momentum component, the other is a requirement on the end-condition \((\mathbf {x}_{1},\mathbf {n}_{1}= R_{\mathbf {n}_{1}}\mathbf {e}_{z})\) which should belong to a set \(\mathcal {R} \subset {\mathbb {R}^{3}\rtimes S^{2}}\) that we express as the range of an exponential map of problem P _{c u r v e} . In fact, this set \(\mathcal {R}\) is precisely the set of end conditions for P _{m e c} where the spatial projection of geodesics does not exhibit a cusp. The formal definition of a cusp will follow in Definition 8. For an illustration of cases \((\mathbf {x}_{1},\mathbf {n}_{1})\not \in \mathcal {R}\) where cusps occur on the spatial projection of sub-Riemannian geodesics, see Fig. 2. The geodesic of P _{m e c} is said to be cuspless if its spatial projection does not have a cusp. The study of cuspless sub-Riemannian geodesics in SE(3) is important for image analysis applications. In particular for the tracking of smoothly varying curvilinear structures (e.g., blood vessels or neural fibers) in noisy 3D (medical) images (see [12, 17, 31]). There, the presence of cusps in the spatial projection of a geodesic is typically undesirable, as it yields a non-smooth path in 3D images. Likewise, to the 2D case [11], it may even be used as a criterium for not connecting two given boundary conditions in an image.

In Section 3, we apply the Pontryagin maximum principle (PMP) [1, 30, 36] to problem P _{M E C} in Section 3.1. In Section 3.2, Theorem 2, we prove Liouville integrability. In Section 3.3, we express the canonical equations of PMP in terms of the − Cartan connection in Theorem 3. Then, a natural choice of SE(3) matrix representation arises in the matrix representation of the Cartan connection, i.e., the Cartan-matrix. We employ this in Theorem 4 containing one of the two key ingredients that we use for integrating the canonical equations of P _{m e c} . The other ingredient is the well-known co-adjoint orbit structure in SE(3) characterized in Lemma 1.

In Section 4, we combine the two ingredients to compute the first cusp-time (Theorem 5 in Section 4.1), and to integrate the canonical equations for geodesics of P _{m e c} . As a result, we obtain, for the first time, explicit analytic formulas for both problems P _{c u r v e} and P _{m e c} . These analytic formulas involve elliptic integrals of the first and the third kind. This is summarized in Theorem 6, which is the main result of this article. Subsequently, in Section 4.3, we derive many geometric properties of the sub-Riemannian geodesics such as:

In Section 5, we conclude with numerical analysis of problem P _{c u r v e} on \(\mathbb {R}^{3}\). Numerical experiments in Section 5.1, see Fig. 7, indicate that the first conjugate time comes after the first cusp time, as in the 2D-case [6]. Numerical experiments in Section 5.2 on the set \(\mathcal {R}\) and the cones of reachable angles, see Fig. 8, put a conjecture on homeomorphic/diffeomorphic properties on the exponential map (cf. Conjecture 1). Finally, we use the analytic formulas for the sub-Riemannian geodesics for numerical solutions to the boundary value problem as briefly explained in Section 5.3. Wolfram Mathematica code for solving the boundary value problem can be downloaded from http://​bmia.​bmt.​tue.​nl/​people/​RDuits/​final.​rar.

A first-order necessary optimality condition is given by PMP [1, 30]. Note that PMP gives a necessary but not a sufficient condition of optimality. Geodesics of P _{M E C} loses local optimality after the first conjugate point and global optimality after the first Maxwell point (see [1]).

The Cauchy-Schwarz inequality implies that the minimization problem for the sub-Riemannian length functional is equivalent to the minimization problem for the action functional (see [1])

Next, we apply PMP to problem P _{M E C} using the equivalent action functional, i.e., to the following optimal control problem:

Denote \(q = (x,y,z,\gamma ,\beta ,\alpha ) \in \mathbb {R}^{3}\times (-\pi ,\pi ] \times [-\frac {\pi }{2}, \frac {\pi }{2}]\times (-\pi , \pi ]\) which is identified with an element from SE(3) via (2.1). The natural momentum coordinates {λ _i } for left-invariant sub-Riemannian problems come along with the left-invariant Hamiltonians \(h_{i}:T^{*}(\text {SE}(3)) \to \mathbb {R}\) (see [1]), given by where \(\lambda (q) = p_{1}(q) \mathrm {d}x|_{q} +p_{2}(q) \mathrm {d}y|_{q} +p_{3}(q) \mathrm {d}z|_{q} +p_{4}(q) \mathrm {d}\gamma |_{q} + p_{5}(q) \mathrm {d}\beta |_{q} + p_{6}(q) \mathrm {d}\alpha |_{q}= {\sum }_{i=1}^{6}\lambda _{i}(q) \, \omega ^{i}|_{q} \in T_{q}^{\ast }(\text {SE}(3))\) denotes a momentum covector expressed in respectively the fixed and the moving dual frame.

Now, we apply PMP. By Remark 4, we only need to consider the normal case. The control-dependent Hamiltonian reads \(H_{u} = u^{3} \lambda _{3} + u^{4} \lambda _{4} + u^{5} \lambda _{5} - \frac {1}{2} \left (\xi ^{2} (u^{3})^{2} +(u^{4})^{2} + (u^{5})^{2}\right )\). Optimization over all controls produces the (maximized) Hamiltonian and gives expression for the extremal controls

By virtue of the Lie brackets (see Table 1), we have the Poisson brackets

Thus, the Hamiltonian system of PMP reads as follows:

These equations only hold outside the singularities of the angles chart {α,β,γ}. In particular, if \(\beta \in \{-\frac {\pi }{2},\frac {\pi }{2}\}\), we can rely on standard Euler angles \(R=R_{\mathbf {e}_{z}, \bar {\gamma }}R_{\mathbf {e}_{y},\bar {\beta }} R_{\mathbf {e}_{z},\bar {\alpha }}\) and in the vicinity of those singularities, the final three equations of the horizontal part need to be replaced by

The sub-Riemannian geodesics are solutions to the Hamiltonian system. Finding a parameterization of the sub-Riemannian geodesics is a nontrivial problem. In order to guarantee that such a parametrization exists, we first prove Liouville integrability of the system. Here, we follow the same approach as in [24].

To prove the Liouville integrability of the Hamiltonian system (3.5), one should construct a complete system of first integrals, i.e., indicate six first integrals in involution (w.r.t. Poisson brackets) and functionally independent on an open dense domain in T ^∗(SE(3)) [2, p. 107].

It is well known that the Hamiltonian \(H = \frac {1}{2}\left ({\lambda _{3}^{2}} + {\lambda _{4}^{2}} + {\lambda _{5}^{2}} \right )\) is a first integral of the Hamiltonian system. From the vertical part of Eq. 3.5, we can immediately see one more first integral λ ₆, which is functionally independent from H. Since {H,λ ₆}=0, we see that the integrals H and λ ₆ are in involution. The Hamiltonian system directly reveals the first integral W=−λ ₁ λ ₄−λ ₂ λ ₅−λ ₃ λ ₆. This integral is a Casimir function, i.e., {W,λ _i }=0,i=1,...,6. Casimir functions are functions on the dual space of the Lie algebra commuting in the sense of Poisson brackets with all left-invariant Hamiltonians. They are universal conservation laws on the Lie group. Connected joint level surfaces of all Casimir functions are coadjoint orbits (see [20, Prop. 7.7]). Since these orbits are always even-dimensional (they are symplectic manifolds), the difference between the dimension of the Lie group and the number of Casimir functions is even. Casimir functions are found by solving {C,λ _i }=0. For polynomial functions C, we can write C with undetermined coefficients as a polynomial of degree 1,2,3, and solve the resulting system of equations algebraically. The second Casimir function in SE(3) is given by \(\mathfrak {c}^{2} = {\lambda _{1}^{2}}+{\lambda _{2}^{2}}+{\lambda _{3}^{2}}\). For details on the Casimir functions, see the work of A.A. Kirillov [19] or the book of V. Jurdjevic [18].

To construct the complete system of first integrals, we consider integrals H, λ ₆ and W and find three more first integrals. Then we show that all six integrals are functionally independent on an open dense domain in T ^∗(SE(3)) and are in involution.

In the sub-Riemannian manifold \((\text {SE}(3), {\Delta }, \mathcal {G}_{\xi })\), the directions \({\mathcal {A}}_{1}\), \({\mathcal {A}}_{2}\), and \({\mathcal {A}}_{6}\) are prohibited in the tangent bundle. To get a better grasp on what this means on the manifold level, we consider principal fibre bundles. We use the minus ‘ −’ Cartan connection [8] to connect the Hamiltonian PMP approach to the Lagrangian reduction approach by Bryant and Griffiths [7]. It provides more intuition and is an important tool toward explicit formulas. As is seen by the following theorem, these curves actually describe parallel transport of the momentum covectors w.r.t. Cartan connections. In the original Cartan and Schouten article [8], three Cartan connections are presented, the + , the −, and 0 connection. Here, we shall rely on the minus − Cartan connection for which the left-invariant vector fields are autoparallel [27], since our geometrical control problem is expressed in left-invariant vector fields. In order to keep track of correct tensorial computations, we deal with the general case ξ>0 in Theorem 3. However, by Remark 5, only the case ξ=1 is considered in the remainder of the article.

It is a partial − Cartan connection that originates from a specific choice of principle fiber bundle. For details, see Appendix B below. Another ingredient in the theorem below is the standard exponential map from Lie algebra to Lie group given by T _(0,I)(SE(3))∋A→e ^A ∈SE(3).

Next, we switch to spatial arclength parameter s, as this is convenient. Recall \(\frac { d}{d s}\) is denoted by a prime. Also recall that s-parametrization is well defined until the spatial projection of a sub-Riemannian geodesic exhibits a cusp (recall Definition 8).

Denote by s _{m a x} the minimum positive value of the parameter s where such a cusp appears. Explicit expression for s _{m a x} in terms of the initial momentum λ(0) will follow (see Eq. 4.5). Next, to find the sub-Riemannian geodesics, we integrate the equation in Theorem 3 via a suitable matrix representation visible in the Cartan-matrix of the Cartan connection. Such a group representation \(m: \text {SE}(3) \to \text {Aut}(\mathbb {R}^{6})\) is given by where \(\sigma _{\mathbf {x}}=\sum \limits _{i=1}^{3} x^{i} A_{3+i} \in \text {so}(3)\), so that σ _x y = x×y, with \(\mathbf {x}=\sum \limits _{i=1}^{3} x^{i} \mathbf {e}_{i}\) and A _{3 + i} ∈so(3) given by

Here, we have σ _{R x} = R σ _x R ⁻¹ and thereby m(g ₁ g ₂) = m(g ₁)m(g ₂) for all g ₁,g ₂∈SE(3). Then with short notation ω ^j = ω ^j | _γ , λ = λ| _γ , dλ=dλ| _γ , and where we represent the covector \(\lambda ={\sum }_{i=1}^{6}\lambda _{i} \left .\omega ^{i}\right |_{\gamma }\) by a row-vector λ=(λ ₁,…,λ ₆). So we see that Eq. 3.10 naturally appears in Eq. 3.8: \(\overline {\nabla }_{\dot {\gamma }}^{*}\lambda = 0 \Leftrightarrow \frac {\mathrm {d} {\boldsymbol {\lambda }}}{\mathrm {d} t} - {\boldsymbol {\lambda }} m(\gamma ^{-1}) \frac {\mathrm {d} m(\gamma )}{\mathrm {d} t} = 0\).

For further details, see Appendix B. These details are not necessary for the remainder of the article, where we only rely on Eqs. 3.7, 3.8, 3.10, and 3.11.

Let us recall the first integrals of the Hamiltonian system and the coadjoint orbits, that we use next for the derivation of explicit formulas for the geodesics.

An arbitrary geodesic in P _{m e c} cannot be extended infinitely in s-parameterization, since in the common case its spatial projection presents a cusp, where spatial arclength s-parametrization breaks down. In fact, for any given initial values of \(\underline {\lambda }^{(1)}(0)\) and \(\underline {\lambda }^{(2)}(0)\), the maximum length s _{m a x} of such a geodesic, where we have κ(s)→∞ as s ↑ s _{m a x} , is given by the following theorem.

In order to integrate the geodesic equations, we apply Theorem 4. This provides the explicit formulas for the sub-Riemannian geodesics, which we present in the next theorem. The sub-Riemannian geodesics are parameterized by elliptic integrals of the first, second, and third kind In our formulas below, we use constants W=−λ ₂ λ ₅−λ ₁ λ ₄, \(\mathfrak {c} = \sqrt {\|\underline {\lambda }^{(1)}\|^{2} - \|\underline {\lambda }^{(2)}\|^{2}+1}\).

Let a stationary curve of P _{c u r v e} in \(\mathbb {R}^{3}\) be given by \({\mathbf {x}} : [0, L] \rightarrow \mathbb {R}^{3}\) parameterized by arclength denoted by s. Let the unit tangent, the unit normal, and the unit binormal for this curve be given by T, N, and B, respectively. Let κ and τ denote curvature and torsion, then the Frenet-Serret equations are

Let us first study the curvature and signed torsion of the spatial projection of the sub-Riemannian geodesics. Let us recall the first integral constants W=−λ ₂ λ ₅−λ ₁ λ ₄ and \(\mathfrak {c} = \sqrt {\|\underline {\lambda }^{(1)}\|^{2} - \|\underline {\lambda }^{(2)}\|^{2}+1}\) in Lemma 1. Furthermore, we have that and therefore

Next, we show that when taking the end conditions to be co-planar, one gets W=0 implying that planar curves are the only cuspless geodesic of problem P _{m e c} connecting these conditions.

The following corollaries relate the planar cuspless sub-Riemannian geodesics in \((\text {SE}(3),{\Delta }, \mathcal {G}_{1})\) to those in \((\text {SE}(2),\tilde {\Delta },\tilde {\mathcal {G}}_{1})\) with \(\tilde {\Delta }=\ker \{-\sin {\theta }\mathrm {d} x + \cos {\theta }\mathrm {d} y\}\) and \(\tilde {\mathcal {G}}_{1}=(\cos {\theta }\mathrm {d} x + \sin {\theta }\mathrm {d} y)\otimes (\cos {\theta }\mathrm {d} x + \sin {\theta }\mathrm {d} y) + \mathrm {d}\theta \otimes \mathrm {d}\theta )\), cf. [11].

Now by the global optimality results in [6, 11], we have the following corollary.

Next, we show that the sub-Riemannian geodesics do not self intersect or roll up, despite the fact that the absolute curvature κ(s)→∞ as s ↑ s _{m a x} .

Next, we want to study bounds on the set \(\mathcal {R}\), recall (2.11). Our numerical investigations clearly show that the spatial part of all points in \(\mathcal {R}\) is contained in the half space z≥0, and that the plane z=0 is only reached with U-shaped planar geodesics (i.e., W=0, \(\mathfrak {c}<1\)) at s = s _{m a x} . These numerical observations inspired us to find the natural generalization of formal results in [11, Thm.7&8] on the SE(2)-case. We partly succeeded as we show in the next three corollaries, which provide bounds on the set \(\mathcal {R}\).

We now describe the symmetries of the exponential map of P _{c u r v e} , recall Definition 6. Here, we are interested in the symmetries that retain curvature and torsion along the curve and preserve direction of time (i.e., we do not consider the symmetries involving time inversion s↦L−s, cf. [25]). From the conservation law and Eq. 4.17 in Theorem 7, we deduce the following corollary.

Figure 6 depicts both the rotational and reflectional symmetries of the cuspless sub-Riemannian geodesics of problem P _{m e c} . To generate the figures, we have set Here, Θ denotes the angle between \(\underline {\lambda }^{(2)}(0)\) and \(\underline {\lambda }^{(1)}(0)\). For both these figures, we fixed \(\|\underline {\lambda }^{(2)}(0)\|\) and \(\|\underline {\lambda }^{(1)}(0)\|\). For Fig. 6a, we took Θ=0 and varied 𝜃. For Fig. 6b, we fixed 𝜃 and varied Θ. The plane of reflection corresponds to Θ = 0. See [16, Fig. 5] for an intuitive explanation of the relation of Θ with respect to the momentum variables.

In this section, we provide a numerical investigation into the absence of conjugate points on sub-Riemannian geodesics associated to the problem P _{c u r v e} . Recall that a conjugate point is a critical value of the exponential map (cf. Definition 6), i.e., at such a point one has \(\det \left (\frac {\partial (Exp(\lambda (0),L))}{\partial (\lambda (0),L)}\right ) = 0\).

Denote by J the Jacobian of the exponential map, i.e., To compute the Jacobian numerically, we approximate the partial derivatives by finite differences:

We verified that the Jacobian is always positive for a million random points within the domain \(\mathcal {D}_{0}\) of the exponential map of P _{c u r v e} , recall (2.10). The points were determined as follows. The first integrals \(\mathfrak {c}^{2} = {\sum }_{i=1}^{3}{\lambda _{i}^{2}}(0)\) and \({\sum }_{i=3}^{5} {\lambda _{i}^{2}}(0) = 1\) allow us to introduce coordinates

By the rotational symmetry presented in Corollary 13, we can reduce one parameter by setting ϕ ₁=0. Furthermore, we recall that λ ₃(0)≥0, which implies \(-\frac {\pi }{2}\leq \phi _{2} \leq \frac {\pi }{2}\). Thus, we can parameterize the domain of exponential map by \(\phi _{2} \in [-\frac {\pi }{2}, \frac {\pi }{2}], \phi _{3} \in [0, 2 \pi ], \mathfrak {c} \in [\cos \phi _{2}, +\infty ), L \in (0, s_{max}]\). We consider \(\mathfrak {c} \in [\cos \phi _{2}, 10]\), and compute the Jacobian in both a random and a uniform grid on \((\phi _{2}, \phi _{3}, \mathfrak {c}, L)\). Here, the restriction of \(\mathfrak {c}\) from above is not crucial, as s _{m a x} →0 when \(\mathfrak {c} \to \infty \). Furthermore, the absence of conjugate points for short arcs of geodesics follows from general theory (see [1]). Finally, by Corollary 3, we have \(s_{max} \leq \frac {1}{2} \log \frac {(1+\mathfrak {c})^{2}}{1 + \mathfrak {c}^{2}}\), that implies s _{m a x} <0.1 for \(\mathfrak {c} > 10\).

In Fig. 7, we show several trajectories of different types (U-shaped curves for \(\mathfrak {c} <1\) and S-shaped curves for \(\mathfrak {c}>1\)) and corresponding plots of the Jacobian for s∈[0,s _{m a x} ]. Remarkably, the Jacobian is not just positive, it is even a monotonically increasing function of s for the range of the plot. A similar behavior for the Jacobian can be seen on the closely related problem P _{c u r v e} on \(\mathbb {R}^{2}\) (see. [11]), where the absence of conjugate points was proved.

There are a number of restrictions on the possible terminal points reachable by sub-Riemannian geodesics of problem P _{m e c} with cuspless spatial projection. Together, such points form the range \(\mathcal {R}\) of the exponential map of P _{c u r v e} , recall (2.11). We present some special cases which help us to get an idea about the range of the exponential map of P _{c u r v e} . Recall that Corollary 12 gave us the possible terminal positions (at z=0) when the final direction is opposite to the initial direction.

Based on our numerical experiments, we pose the following conjecture which is analogous to a result in the 2D case of finding cuspless sub-Riemannian geodesics in \((\text {SE}(2),\tilde {\Delta },\tilde {G}_{1})\) [6, 11].

Note that \(S_{B} \in \mathcal {R}\) and \(S_{R} \in \mathcal {R}\) but \(S_{L} \not \in \mathcal {R}\). This conjecture would imply that no conjugate points arise within \(\mathcal {R}\) and the problem P _{c u r v e} (1.1) is well posed for all end conditions in \(\mathcal {R}\). The proof of this conjecture would be on similar lines as in Appendix F of [11]. If the conjecture is true, we have a reasonably limited set of possible directions per given end positions for which a cuspless sub-Riemannian geodesic of problem P _{c u r v e} exists. Then the cones of admissible end conditions for P _{c u r v e} (recall Definition 7) in Fig. 8a form the image of the boundary of the phase space of \(\{\underline {\lambda }^{(2)}(0), \underline {\lambda }^{(1)}(0)\}\) under the exponential map. Recall Fig. 5. These cones represent the boundary of the possible reachable angles by stationary curves of problem P _{c u r v e} . Figure 8b shows the special case of the end conditions being on a unit circle containing the z-axis. The final tangents are always contained within the cones at each position. Numerical computations indeed seem to confirm that this is the case (see Fig. 9). The blue points on the boundary of the cones correspond to S _B while the red points correspond to S _R given in Eq. 5.1.

Using explicit formulas for sub-Riemannian geodesics obtained in Theorem 6 in Section 4.2, we developed a Wolfram Mathematica package, that numerically solves the boundary value problem (BVP) associated to P _{c u r v e} . The package is available by link http://​bmia.​bmt.​tue.​nl/​people/​RDuits/​final.​rar. Note, that the BVP can be also tackled by a software Hampath [11] developed to solve optimal control problems. Hampath is based on numerical integration of a Hamiltonian system of PMP and second-order optimality conditions. In contrast to Hampath, our program does not involve numerical integration, and is based on numerical solution of a system of algebraic equations in Theorem 6, and relies on shooting based on the exact formulas, for details see [16].