Task-Induced Symmetry and Reduction in Kinematic Systems with Application to Needle Steering

Abstract

Lie group symmetry in a mechanical system can lead to a dimensional reduction in its dynamical equations. Typically, the symmetries that one exploits are intrinsic to the mechanical system at hand, e.g. invariance of the system’s Lagrangian to some group of motions. In the present work we consider symmetries that arise from an extrinsic control task, rather than the intrinsic structure of configuration space, constraints, or system dynamics. We illustrate this technique with several examples. In the examples, the reduction enables us to design essentially global feedback controllers on the reduced systems.

We apply task-induced symmetry and reduction to a recently developed 6 DOF kinematic model of steerable bevel-tip needles. The resulting controllers cause the needle tip to track a subspace of its configuration space. We envision that the methodology presented in this paper will form the basis for a new planning and control framework for needle steering.

I. INTRODUCTION

Consider the motion of a simple cart in a plane, described by the usual kinematic equations of motion (i.e. ẋ = υ cos θ, ẏ = υ sin θ, θ̇ = ω). Suppose we are interested in making the cart follow a line, for example the x-axis. There is a natural symmetry induced by the task: any motion along the x-axis does not affect the control task. Hence to follow the x-axis, we only need to consider two of the three states and their dynamics, namely ẏ = υ sin θ, θ̇ = ω. In this trivial example, reduction results from an extrinsic, “user-defined” control task, rather than an intrinsic property of the system itself.

For the trivial example above, it is not clear if a similar reduction would be possible if the goal were to follow some other shape, such as, say, a circle, or if the kinematics were more complicated. In the following sections we present a general framework in which to answer these questions and then illustrate the approach with several examples on SE(2) and SE(3) that culminate in a novel approach to the control of steerable needles, which was the principal motivation for us to study this problem. Our approach involves two simple ingredients. First, for simplicity and since the primary motivation for the present work is needle steering which has been modeled as a kinematic system, we consider kinematic systems on Lie groups. Second, the task should be described as the zero of a “task functional” on the group. With these ingredients, the subgroup of configuration space to which the task is invariant is used to reduce the kinematic equations of motion.

In previous work on mechanical systems reduction has been used when the dynamics of the system exhibit some intrinsic symmetry. If the Lagrangian of a mechanical system is invariant to the action of a Lie group, then such a system is said to exhibit symmetry and the Lie group is known as the symmetry group. In unconstrained systems with symmetry, mechanical connections as defined in [11] can be used for reduction. Bloch et al. [4] define a nonholonomic connection as a combination of mechanical and Ehresmann connections and use it to perform Lagrangian reduction in the presence of nonholonomic constraints. Ostrowski [13] uses these connections to reduce the systems whose configuration spaces are a direct product of the symmetry group (also called the fiber) and a manifold called a shape space (also called the base space).

The work we present in this paper is closely related to the literature on relative equilibria [5], [8] however the systems we consider in this paper are kinematic systems. Roughly speaking, given a Hamiltonian system with symmetry, a relative equilibrium is a trajectory that is an equilibrium of the reduced system. However, our primary system of interest in the present work, needle steering, is inherently highly symmetric; in fact, the needle evolves according to a left-invariant vector field on SE(3), rendering SE(3) itself the intrinsic symmetry group! That observation itself does not prove particularly useful from a design point of view, so we introduce tasks that actually break some of the symmetry. Then, the “left over” symmetry enables task-specific reduction and leads us, at least in the specific examples presented, to an essentially global control strategy to achieve the task.

II. GENERAL FRAMEWORK

Let G be a Lie group.¹ Consider the left-invariant kinematic system

$$g^{-1}\stackrel{.}{g}={\xi }_0+{\displaystyle \sum _{i=1}^k{\xi }_i{u}_i}$$ (1)

where g = g(t) ∈ G is the configuration to be controlled, ξ_i ∈ 𝔤 are constant vectors in the Lie algebra of G, k is the total number of scalar inputs to the system and u_i’s for i = 1, 2, ‥k are inputs to the system. We consider cases where the control task is encoded as the zero value of a functional φ : G → ℝ defined on the configuration space. Let H be the subgroup² of G invariant to the task, namely

$$H=\{h\in G:\forall g\in G,\phi (\mathit{\text{hg}})=\phi (g)\}.$$ (2)

Our goal is to find a (feedback) controller u such that φ(g(t)) → 0 asymptotically.

Intuitively, we ignore motions in the space H, since such motions get us no closer to or further from our goal of bringing φ(g) to zero. In effect, we perform control only in the space B that is “left over” after ignoring H. Since B is of lower dimension than G (dim B = dim G−dim H), the kinematics (1) and task functional φ likely have a simpler form on B than on G. In fact, in the examples that follow, the task functional reduces to a candidate Lyapunov function.

Formally, we use H and G to construct a fiber bundle (see Appendix). Since H is a subgroup of G there exists a bundle projection π : G → B := G/H. Since H does not affect the control task, we consider the state evolution only in the base space. We use r to represent coordinates for the base space B and h for those in the fiber H. Since the vector field in (1) is G invariant and H is a subgroup of G, the vector field is also H invariant. This results in a well defined vector field on B that does not depend on H. The vector field on B can be calculated as

$$\stackrel{.}{r}=f_0(r)+{\displaystyle \sum _{i=1}^k{f}_i(r)u_i}$$ (3)

where f_i(r) = Tπ · gξ_i. This is the reduced kinematic equation on the base space B.

For completeness, we derive the kinematic equation for the fiber variable h ∈ H. Though one can employ the concept of connections for a global derivation, we content ourselves with the derivation in local coordinates.³ Consider a local section s : U ⊂ B → G. Recall that by definition, a local section s satisfies π ○ s = Id_U⁴. This section, s, induces the local trivialization which is a local diffeomorphism defined by the map:

$$\psi :H\times U\to {\pi }^{-1}(U),(h,r)\to h·s(r).$$

Its inverse ψ⁻¹ : π⁻¹(U) → H × U is given by ψ⁻¹(g) = (g · (s(π(g)))⁻¹, π(g)). From the definition of ψ we can rewrite g⁻¹ġ as

$$\begin{array}{cc}{g}^{-1}\stackrel{.}{g}\hfill & ={(hs(r))}^{-1}\frac{d}{\mathit{\text{dt}}}(hs(r))\hfill \\ \hfill & ={\text{Ad}}_{s(r)}^{-1}{h}^{-1}\stackrel{.}{h}+s{(r)}^{-1}\mathit{\text{Ts}}\cdot \stackrel{.}{r}.\hfill \end{array}$$

This results in

$$h^{-1}\stackrel{.}{h}={\text{Ad}}_{s(r)}\left({\xi }_0+{\displaystyle \sum _{i=1}^k{\xi }_i{u}_i-s{(r)}^{-1}\mathit{\text{Ts}}\cdot \stackrel{.}{r}}\right)$$ (4)

which also depends only on r and u. The equation for the fiber variable h in (4) is sometimes called the reconstruction equation [10]. The above construction can be summarized in a commutative diagram:

We illustrate this reduction method by applying it to the tasks of following lines and circles with a planar cart. Then, we tackle the problem of steering flexible bevel-tip needles to follow spheres and planes in ℝ³.

III. FOLLOWING CURVES WITH A PLANAR CART

In this section we revisit the motivating example from Section I and apply the above reduction framework. We then show how this can be applied to a more interesting problem of following a circle. These toy problems are useful because they have similar (but simpler) structure to that of the needle steering problems discussed in Section IV.

Consider a body reference frame attached to the cart at (x, y) with the frame x-axis pointing along the forward translational velocity of the cart. The configuration space G is the group of transformations in 2D, namely G = SE(2) ≅ SO(2)Ⓢℝ² ≅ S¹Ⓢℝ². To denote an element g ∈ SE(2), we use both g = (R, p) ∈ SO(2)Ⓢℝ² and its homogeneous representation

$$g=\left[\begin{array}{cc}\hfill R\hfill & \hfill p\hfill \\ \hfill 0^T\hfill & \hfill 1\hfill \end{array}\right],\text{where}R=\left[\begin{array}{cc}\hfill \text{cos}\theta \hfill & \hfill -\text{sin}\theta \hfill \\ \hfill \text{sin}\theta \hfill & \hfill \text{cos}\theta \hfill \end{array}\right],p=\left[\begin{array}{c}\hfill x\hfill \\ \hfill y\hfill \end{array}\right].$$ (5)

The kinematic equation of the unicycle can be written as

$$g^{-1}\stackrel{.}{g}=\left[\begin{array}{cc}\hfill \widehat{\mathrm{\Omega }}\hfill & \hfill \left[\begin{array}{c}\hfill \upsilon \hfill \\ \hfill 0\hfill \end{array}\right]\hfill \\ \hfill 0\hfill & \hfill 0\hfill \end{array}\right],\text{where}\widehat{\mathrm{\Omega }}=\left[\begin{array}{cc}\hfill 0\hfill & \hfill -\omega \hfill \\ \hfill \omega \hfill & \hfill 0\hfill \end{array}\right].$$ (6)

In this case the configuration space G is the special Euclidean group in two dimensions i.e. G = SE(2). We now consider two control tasks: following a line and following a circle.

A. Straight Line Following

To follow the x-axis, the cart’s position should be such that it is oriented parallel to the x-axis and its y-coordinate should be zero. This is precisely the example discussed in the introduction, and as shown, is trivial in local coordinates on SE(2); because of its simplicity, it may seem that the machinery in this paper is more complicated than needed, but the example serves to illustrate the general framework in detail.

For the case when the cart is following the x-axis in the positive direction, this task can be defined as a zero of the function φ defined as

$$\phi (g)=\phi (R,p)=(1-e_1^T{\mathit{\text{Re}}}_1)+\frac{1}{2}{(e_2^{T}p)}^2,$$

where e_i’s for i = 1, 2 are principal unit vectors in two dimensions. When the cart is following the x-axis in the negative direction, this task can be defined as a zero of the function, $\phi (R,p)=(1+e_1^T{\mathit{\text{Re}}}_1)+\frac{1}{2}{(e_2^{T}p)}^2$.

The function φ is invariant to translations along the x-axis of the world frame. Hence given this control task, the symmetry group is

$$\begin{array}{cc}H\hfill & =\{h\in \text{SE}(2):\forall g\in \text{SE}(2),\phi (\mathit{\text{hg}})=\phi (g)\}\hfill \\ \hfill & =\left\{h\in \text{SE}(2)|h=\left[\begin{array}{ccc}1\hfill & 0\hfill & x\hfill \\ 0\hfill & 1\hfill & 0\hfill \\ 0\hfill & 0\hfill & 1\hfill \end{array}\right],x\in ℝ\right\}\cong ℝ.\hfill \end{array}$$

Let B := G/H ≅ S¹ × ℝ. Define the projection map, π : G → B by

$$\pi (g)=(R^{-1}{e}_1,e_2^{T}p).$$

For all h ∈ H, $\pi (\mathit{\text{hg}})=\pi (R,p+{\mathit{\text{xe}}}_1)=(R^{-1}{e}_1,e_2^{T}p)=\pi (g)$. Therefore, π is H invariant. Also, if g₁ ∈ π⁻¹(r) then g₂ = hg₁ ∈ π⁻¹ (r), ∀h ∈ H. Therefore π⁻¹ (r) ≅ H for each r ∈ G/H. Let r = [r₁, r₂, r₃]^T denote an element in B with $r_1^2+r_2^2=1$. Define a global section s : B = S¹ × ℝ → SE(2) by

$$s(r)=\left[\begin{array}{ccc}\hfill r_1\hfill & \hfill r_2\hfill & \hfill r_3\hfill \\ \hfill -r_2\hfill & \hfill r_1\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right].$$

Note that π ○ s = Id_B. Hence, the projection π : SE(2) → S¹ × ℝ is a trivial bundle. With this section, a global diffeomorphism ψ : H × B ≅ ℝ × (S¹ × ℝ) → SE(2) can be constructed by ψ(h, r) = hs(r). This fiber bundle can be represented by the following diagram:

Assuming that the forward velocity of the cart, υ, is held constant at υ = 1, the dynamics in the reduced space can be calculated as:

$$\stackrel{.}{r}=\left[\begin{array}{c}\hfill 0\hfill \\ \hfill 0\hfill \\ \hfill -r_2\hfill \end{array}\right]+\left[\begin{array}{c}\hfill r_2\hfill \\ \hfill -r_1\hfill \\ \hfill 0\hfill \end{array}\right]\omega$$ (7)

with a holonomic constraint given by $r_1^2+r_2^2=1$. The reconstruction equation for the fiber variable h = x is given by

$$\stackrel{.}{x}=r_1.$$ (8)

Observe that R⁻¹ e₁ is the vector e₁ seen in the body reference frame. If the cart is following the x-axis, r = [±1, 0, 0]^T depending on whether the cart is following the line along the positive or negative x-axis. We claim that choosing the following control input will result in the cart following the desired line:

$$\omega ={\mathit{\text{kr}}}_2-r_3\text{ for some }k\in {ℝ}^{+}.$$ (9)

We use the task function reduced to the base space as a candidate Lyapunov function, $\phi (r)=1-r_1+\frac{1}{2}{r}_3^2$. The time derivative of φ, upon plugging (9), is $\stackrel{.}{\phi }(r)=-{\mathit{\text{kr}}}_2^2$ which is negative semi-definite. Each level set Ω_c = {r ∈ B : φ(r) ≤ c}, c > 0 is positive invariant. The subset where φ̇ = 0 is given by E = {r ∈ S¹ × ℝ : r₂ = 0} ⊂ Ω_c. The largest invariant subset in E contains only the points r = [0,±1, 0]^T. Therefore from LaSalle’s invariance principle, we conclude that the cart follows the desired line. Further by considering the local chart (r₂, r₃) and by eigen-value analysis we can see that the equilibrium point r = [0, 1, 0]^T is stable and the other one is a saddle. To follow the line in the other direction, simply let ω = r₃ − kr₂ for some k ∈ ℝ⁺.

B. Circle Following

Suppose we are interested in making the cart follow a circle given by X = {(x, y) ∈ ℝ² | x² + y² − ρ² = 0}. This task can also be described as the zero of the function, φ defined by:

$$\phi (g)=\phi (R,p)=\frac{1}{2}{\Vert \rho e_2-R^{-1}p\Vert }^2.$$

One can show that H = {g = (R, p) : p = 0} ≃ SO(2) ⊂ SE(2) is the largest subgroup that leaves φ invariant. Construct a fiber-bundle π : SE(2) → SE(2)/SO(2) by

$$\pi (g)=\pi (R,p)=R^{-1}p.$$

Notice that B := SE(2)/SO(2) ≅ ℝ². If we introduce a global section s : ℝ² → SE(2) by

$$s(r)=\left[\begin{array}{cc}\hfill I_{2\times 2}\hfill & \hfill r\hfill \\ \hfill 0\hfill & \hfill 1\hfill \end{array}\right]$$

for r ∈ ℛ, then we can see that π defines a trivial bundle. Let r = [r₁, r₂]^T. Assuming that the forward velocity of the cart is constant (say, υ = 1), the reduced space dynamics are given by

$$\stackrel{.}{r}=R^{-1}\stackrel{.}{p}-R^{-1}\stackrel{.}{R}r=\left[\begin{array}{c}\hfill 1\hfill \\ \hfill 0\hfill \end{array}\right]+\left[\begin{array}{cc}\hfill 0\hfill & \hfill 1\hfill \\ \hfill -1\hfill & \hfill 0\hfill \end{array}\right]r\omega$$ (10)

and the fiber variable θ satisfies

$$\stackrel{.}{\theta }=\omega .$$ (11)

Observe that r = R⁻¹p is the vector from the center of the desired circle to the origin of the body frame, written in the body frame. This implies that if the cart is following the desired circle clockwise r = [0, ρ]^T, and r = [0,−ρ]^T if it is following the desired circle counter-clockwise. We show that choosing the following control input will result in the cart following the desired circle clockwise:

$$\omega =-\frac{1+r_{1}k}{\rho },\text{ for some }k\in {ℝ}^{+}.$$ (12)

Again we use the task function (restricted to the base space) as a candidate Lyapunov function, namely $\phi (r)=\frac{1}{2}{r}_1^2+\frac{1}{2}{(r_2-\rho )}^2$. Then the time derivative of the Lyapunov function is $\stackrel{.}{\phi }(r)=r_1(1+r_2\omega )+(r_2-\rho )(-r_1\omega )=-{\mathit{\text{kr}}}_1^2$. We note that φ̇(r) is negative semi-definite and that the invariant subset of the set where φ̇(r) = 0 consists of only r = [0, ρ]^T. As before, one can show using LaSalle’s invariance principle that the cart follows the desired circle clockwise. Figure 1 is a simulation of the circular wall-following in a cart. The cart is initially away from the desired circle but, it asymptotically follows the desired circle. If it is desired to follow the circle counter-clockwise, choose the input as $\omega =\frac{1+\mathit{\text{rk}}}{\rho }$. Note here that convergence is global.

Fig. 1.

Cart following a circle: Red dotted circle is the desired circle to be followed by the cart. Initially the cart is away from the desired circle but with the application of the controller, it follows the circle.

IV. APPLICATIONS TO NEEDLE STEERING

Long thin needles are used to perform percutaneous therapies in many diagnostic and therapeutic procedures. It has been shown that flexible bevel-tip needles have better maneuverability than rigid symmetric needles and thus improve the physicians’ performance [6], [14]. A flexible bevel-tip needle can be steered by rotation and insertion at the base of the needle (outside the patient). During insertion, the asymmetry of the bevel creates a moment at the needle tip, deflecting the needle and causing it to follow a circular arc. As the needle is rotated, the bevel tip is reoriented in space, so that subsequent insertion follows an arc in a new plane.

We use the kinematic model developed by Webster et al. [14] for a bevel tip flexible needle. It is modeled as a generalisation of the nonholonomic bicycle model, and neglects torsional compliance of the needle shaft (Figure 2). In this model, ℓ₁, ℓ₂ determine the location of bicycle wheels with respect to the needle tip. Parameter ϕ is the fixed front wheel angle relative to the rear wheel. Frame A is the inertial world reference frame and frames B and C are attached to the two wheels of the bicycle. In homogeneous coordinates, the rigid body transformation between frames A and B is given by the rigid body transformation matrix:

$$g=\left[\begin{array}{cc}\hfill R\hfill & \hfill p\hfill \\ \hfill 0^T\hfill & \hfill 1\hfill \end{array}\right]\in \text{SE}(3)\text{\hspace{1em}where\hspace{1em}}R\in \text{SO}(3)\text{and}p\in {ℝ}^3.$$ (13)

Webster et al. obtain a kinematic model for needle steering:

$$g^{-1}\stackrel{.}{g}={\xi }_1\upsilon +{\xi }_2\omega ,$$

where υ is the insertion speed, ω is the rotation speed of the needle, and the control vector fields are given by ${\xi }_1^{\vee }={[001\kappa 00]}^T\text{and}{\xi }_2^{\vee }={[000001]}^T$.

Fig. 2.

Needle steering kinematic model: Frame A is the inertial world reference frame. Frames B and C are attached to the two wheels of the bicycle. This figure is reproduced from [14] with permission from the authors.

If the needle is inserted into the tissue without any rotation at the base, it stays in a 2D plane within the tissue. The location of the 2D plane depends on the initial insertion conditions of the needle into the tissue. Also, in the 2D plane the needle follows a circle of curvature κ = tan ϕ/ℓ₁.

A. Motivation: Path Planning on Subsurface Patches

The high degree of nonholonomy for needle steering (four [14]) renders global feedback control to a point in SE(3) challenging, and likely impossible in practice. One possible solution is to plan a path in SE(3) and execute feedback control to follow the path. However, feasible paths are not necessarily asymptotically trackable. In fact, following the natural trajectory of the system, a circle of radius 1/κ, is not small-time locally trackable, in the sense that there exist arbitrarily small perturbations cannot be driven back to the desired trajectory in an arbitrarily small amount of time. To see this, refer to Figure 3.

Fig. 3.

Small-time local controllability fails even for tracking a natural trajectory of the needle, a circle (green, dashed) of radius 1/κ. Here, a small initial perturbation in heading of the needle (black, solid) is depicted. The radius of curvature of the actual needle trajectory can never be less than 1/κ, and thus even for the smallest possible perturbation of the needle heading, the needle must travel at least π/κ before re-intersecting (*) the desired path. Pitching out of the plane of the circle only exacerbates this issue, and the problem cannot be overcome with control or replanning.

There are several potential ways to overcome this problem. One could plan paths whose radius of curvature is always greater than 1/κ. We suspect such curves can be followed arbitrarily closely (if not exactly), although the high degree of nonholonomy of the system may render this challenging.

An alternative approach would be to develop low-level controllers that cooperate with the 2D planners by Alterovitz et al. [1], [2]. Their planner, which relies on the needle staying within a specified 2D plane, constructs a sequence of circular arcs of radius 1/κ that can be achieved via alternating insertions and 180° rotations of the needle shaft. A real-time replanner corrects for within plane deviations from the desired piecewise circular path. In the prior work, Kallem and Cowan [9] designed and implemented in simulation and laboratory experiments an observer-controller pair that steers a needle to stay within a desired 2D plane. In effect, this low-level 2D plane-following controller ensures that the needle remains close to a desired 2D plane, on top of which Alterovitz et al.’s planner can operate.

Our long-term research will generalize this real-time replanning and low-level control architecture, as depicted in Figure 4. We envision a “subspace planner” that chooses feasible subspaces, upon which an Alterovitz-style planner [1] builds paths within each subspace.

Fig. 4.

A stylized depiction of a needle trajectory that remains within a sequence of 2D patches. The plane-following controller was reported in [9], and the sphere-following problem is reported in this paper (Section IV-C).

It is important to understand how the planning and control architecture relates to the methodology outlined in Section II. In needle steering, the entire space G is all of SE(3) – positions and orientations of the needle tip in the tissue. The space in which the planner acts – that is, motions within a surface – is the symmetry group, or fiber, that defines the projection from the entire space into the base space. The base space is where the low-level controller operates in an effort to drive the needle back to the desired surface.

The prior work by Kallem and Cowan [9] used local coordinates to parameterize the configuration space (SE(3)), and performed a coordinate-based reduction “by hand” by noting that the three configuration variables central to the control task evolved independently of the others. Here, we apply the general framework developed in Section II for two additional tasks.

B. Controlling the Needle Tip to a Plane Parallel to a Desired 2D Plane

Without any loss of generality, let the desired plane be the y − z plane. The configuration space, G is SE(3). The control task is to stabilize the needle tip to a plane parallel to the y−z plane. In other words, it is desired that the circle the needle tip traces at steady state, during insertion without any rotation at the base be parallel, to the y − z plane. The task can be described by a surface X ⊂ SE(3) given by

$$X=\{x\in \text{SE}(3)|x=(e^{{ê}_1\theta },p)\text{where}\theta \in ℝ\text{and}p\in {ℝ}^3\}.$$

The task can also be described as the zero of the function, φ defined as:

$$\phi (g)=\phi (R,p)=1-e_1^T{\mathit{\text{Re}}}_1$$

where g ∈ SE(3) is represented as in (13) and e_i’s for i = 1, 2, 3 are the principal unit vectors in three dimensions. This form of the function φ is for the cases when the needle tip in steady state undergoes rotation about the positive x-axis. In the case when the needle tip undergoes rotation about the negative x-axis, the function, φ can be defined as $\phi =\phi (R,p)=1+e_1^T{\mathit{\text{Re}}}_1$.

Note that X is also a subgroup of G = SE(3). Hence the symmetry group is

$$\begin{array}{cc}H\hfill & =\{h\in \text{SE}(3):\forall g\in \text{SE}(3),\phi (\mathit{\text{hg}})=\phi (g)\}\hfill \\ \hfill & =X\cong \text{SO}(2)\mathrm{Ⓢ}{ℝ}^3\cong \text{SE}(2)\times ℝ.\hfill \end{array}$$

The symmetry group, H, can be thought of as a combination of 2D transformations in the y − z plane and translations along the x-axis relative to the world frame. With these group definitions, B := G/H = S². Define a projection map from G to B as

$$\pi (g)=\pi (R,p)=R^{-1}{e}_1.$$

Choose an open subset U of B as the unit sphere excluding the (−1, 0, 0) point. That is U = S² − {(−1, 0, 0)}. Define the bundle section on U, s : U → π⁻¹ (U) ⊂ G as

$$s(r)=(e^{\widehat{\varpi }(r)\pi },0),$$

where $\varpi (r)=\frac{e_1+r}{\Vert e_1+r\Vert }$. This section can be seen in Figure 5. With this section, locally g ∈ G can be expressed as g = hs(r) ≃ (h, r), where h ∈ H and r ∈ B. To completely cover S² consider U′ = (S² − {(1, 0, 0)}) and a similar section can be constructed. These make the map π a fiber bundle which can be summarized by the following commutative diagram:

Fig. 5.

Illustration of the Bundle Section: Unit vector ϖ bisects r and e₁. If the r is rotated about ϖ as axis by π then it coincides with e₁. The circle through the points r and (1, 0, 0) is a latitude of S² drawn perpendicular to ϖ.

We assume that the insertion speed is held constant at υ = 1. This assumption is equivalent to parameterization of the system with the length of the needle inserted in to the tissue (when the insertion speed is not zero). Base space elements can be written in coordinates as r = [r₁, r₂, r₃]^T ∈ ℝ³ with $r_1^2+r_2^2+r_3^2=1$. The dynamics of the base space can be calculated as:

$$\stackrel{.}{r}=\left[\begin{array}{c}\hfill 0\hfill \\ \hfill {\kappa r}_3\hfill \\ \hfill -{\kappa r}_2\hfill \end{array}\right]+\left[\begin{array}{c}\hfill r_2\hfill \\ \hfill -r_1\hfill \\ \hfill 0\hfill \end{array}\right]\omega .$$ (14)

The fiber variable h = (e^ê₁θ, p) ∈ H satisfies

$$\begin{array}{cc}\stackrel{.}{\theta }{ê}_1\hfill & ={\text{Ad}}_{\overline{s}(r)}\left(\kappa {ê}_1+\omega {ê}_3-\overline{s}{(r)}^{-1}\frac{\partial \overline{s}(r)}{\partial r}\stackrel{.}{r}\right)\hfill \\ \hfill \stackrel{.}{p}& =e^{{ê}_1\theta }{e}^{\widehat{\varpi }(r)\pi }{e}_3,\hfill \end{array}$$ (15)

where s̄(r) = e^ϖ̂(r)π.

For the task to be completed, the state in the reduced system should be r = [±1, 0, 0]^T. Choosing the following control input we show that the task is achieved:

$$\omega ={\mathit{\text{kr}}}_2,\text{ for some }k\in {ℝ}^{+}.$$ (16)

Once again, we use the task function in base coordinates as a candidate Lyapunov function, namely φ(r) = 1 − r₁. The time derivative of the Lyapunov function is $\stackrel{.}{\phi }(r)=-{\mathit{\text{kr}}}_2^2$ which is negative semi-definite in r. The set where φ̇ = 0 is given by E = {r = [r₁, r₂, r₃]^T ∈ S²|r² = 0}. The largest invariant set in E contains only the points r = [±1, 0, 0]^T. Hence, from LaSalle’s invariance principle, we can conclude that needle tip can be stabilized to a plane parallel to the desired plane. Notice that asymptotically the fiber dynamics become

$$\begin{array}{c}\stackrel{.}{\theta }=\kappa ,\hfill \\ \stackrel{.}{p}={(0,\text{sin}\theta ,-\text{cos}\theta )}^T.\hfill \end{array}$$

This shows us that the fiber variables trace a circle of radius 1/κ, which we knew from the needle steering model. Further by considering the local chart (r₂, r₃) and by eigen-value analysis we can see that the equilibrium point r = [1, 0, 0]^T (corresponding to the needle tip tracing a circle of radius 1/κ parallel to the y − z plane counter clockwise) is stable, and the other equilibrium point unstable. For the needle tip to trace the circle clockwise, let ω = −kr₂ for some k ∈ ℝ⁺.

C. Controlling the Needle Tip to a Spherical Surface

In this section we use the reduction technique developed in II to control the needle tip to the surface of a sphere with radius ρ > 1/κ. If the needle tip stays on the surface of the desired sphere at steady state it has to follow a circle of radius ϱ = 1/κ. Let $d=\sqrt{{\rho }^2-{\varrho }^2}$. If the needle is staying on the desired circle on the sphere, the position vector of the center of the sphere written in the body frame is given by R⁻¹p. This must be [−d, ϱ, 0]^T or [d, ϱ, 0]^T depending on which direction the needle tip is following the circle of radius ϱ.

In this example, the control task can also be described as the zero of the function

$$\phi (g)=\phi (R,p)={\Vert R^{-1}p-{[-d,\varrho ,0]}^T\Vert }^2$$

or

$$\phi (g)=\phi (R,p)={\Vert R^{-1}p-{[d,\varrho ,0]}^T\Vert }^2$$

depending on the direction the needle tip follows the circle. It is easy to see that H = SO(3) leaves the function, φ, invariant. Hence, the symmetry group, H for this control task is SO(3). The base space, B := G/H = ℝ³. The bundle projection π : SE(3) → ℝ³ is given by

$$\pi (g)=R^{-1}p$$

where g = (R, p) ∈ SO(3)Ⓢℝ³ ≅ SE(3). This bundle is trivial because we can construct a global section s : ℝ³ → SE(3) as

$$s(r)=\left[\begin{array}{cc}\hfill I_{3\times 3}\hfill & \hfill r\hfill \\ \hfill 0^T\hfill & \hfill 1\hfill \end{array}\right].$$

Given (R, r) ∈ H × B = SO(3) × ℝ³, the corresponding g ∈ SE(3) can be calculated as $g=\left[\begin{array}{cc}\hfill R\hfill & \hfill \mathit{\text{Rr}}\hfill \\ \hfill 0\hfill & \hfill 1\hfill \end{array}\right]$. Assuming υ = 1, the dynamics in the base space can be written as

$$\stackrel{.}{r}=\left[\begin{array}{c}\hfill 0\hfill \\ \hfill {\kappa r}_3\hfill \\ \hfill 1-{\kappa r}_2\hfill \end{array}\right]+\left[\begin{array}{c}\hfill r_2\hfill \\ \hfill -r_1\hfill \\ \hfill 0\hfill \end{array}\right]\omega$$ (17)

and the fiber variable R ∈ H = SO(3) satisfies

$$R^{-1}\stackrel{.}{R}=\kappa {ê}_1+{ê}_3\omega .$$ (18)

The control input

$$\omega =-k({\mathit{\text{dr}}}_2+\varrho r_1)\text{ for some }k\in {ℝ}^{+}$$ (19)

makes the reduced space converge to r = [−d, ϱ, 0]^T. Consider the reduced task functional as the candidate Lyapunov function: $\phi (r)=\frac{1}{2}{(r_1+d)}^2+\frac{1}{2}{(r_2-\varrho )}^2+\frac{1}{2}{r}_3^2$. Its time derivate, φ̇(r) = ω(dr₂ + ϱr₁) = −(dr₂ + ϱr₁)², is negative semi-definite. The invariant subset of the set where φ̇(r) = 0 contains only r = [−d, ϱ, 0]^T. Again using LaSalle’s invariance principle we can conclude that the needle tip stays on the surface of the desired sphere. To follow the circle in the desired sphere in the other direction, choose ω = dr₂ − ϱr₁.

V. CONCLUSIONS

We developed a general framework to perform task-specific reduction in kinematic systems on Lie groups. We applied this method to a two systems, a planar cart (as an illustrative example) and flexible bevel-tip needle steering with configuration spaces SE(2) and SE(3) respectively. In the examples, the reduction enabled us to design essentially global feedback controllers on the reduced systems. Also, the task functional became a Lyapunov function in reduced coordinates. We envision that the methodology presented in this paper will form the basis for a new planning and control framework for needle steering.

This paper considered only kinematic systems, but we believe our results can be used along with reduction techniques for mechanical systems (e.g. [13]) to perform a two-stage reduction. In the first stage, mechanics-based “intrinsic” reduction can separate the configuration space into a shape space and a symmetry (Lie) group. The dynamics on the shape space can be obtained from the so-called reduced Lagrangian, while a reconstruction equation generates motions in the symmetry group. This reconstruction equation takes the place of the Lie group kinematics (1) in this paper. Certain control tasks may enable us to perform a second “extrinsic” (task-specific) reduction, thus further decomposing the symmetry group into a base and fiber, as described in this paper.

APPENDIX

A. Lie Groups

A Lie group, is essentially a smooth group which is also a smooth manifold. The groups discussed in this paper in detail are the Euclidean groups SE(2) = SO(2)Ⓢℝ² and SE(3) = SO(3)Ⓢℝ³. The symbol Ⓢ denotes semi-direct product. If R₁,R₂ ∈ SO(2) and p₁, p₂ ∈ ℝ², the product in the group obtained by semi-direct product of SO(2) and ℝ² is given by (R₁, p₁) * (R₂, p₂) = (R₁R₂,R₁p₂ + p₁). Note that as sets, SE(2) = SO(2) × ℝ² but as groups, SE(2) = SO(2)Ⓢℝ².

We identify the tangent space TSE(3) of the Lie group SE(3) with

$$T\text{SE}(3)\simeq \text{SE}(3)\times 𝔰𝔢(3),$$ (20)

where 𝔰𝔢(3) is the Lie algebra of SE(3). The “wedge/hat” isomorphism ℝ³ ≃ 𝔰𝔬(3) is defined by

$$^:\left[\begin{array}{c}\hfill {\mathit{\omega }}_1\hfill \\ \hfill {\mathit{\omega }}_2\hfill \\ \hfill {\mathit{\omega }}_3\hfill \end{array}\right]\leftrightarrow \left[\begin{array}{ccc}\hfill 0\hfill & \hfill -{\mathit{\omega }}_3\hfill & \hfill {\mathit{\omega }}_2\hfill \\ \hfill {\mathit{\omega }}_3\hfill & \hfill 0\hfill & \hfill -{\mathit{\omega }}_1\hfill \\ \hfill -{\mathit{\omega }}_2\hfill & \hfill {\mathit{\omega }}_1\hfill & \hfill 0\hfill \end{array}\right]{:}^{\vee }$$

where 𝔰𝔬(3) is the Lie algebra of SO(3). In a standard abuse of notation the wedge/hat isomorphism ℝ³Ⓢℝ³ ≃ 𝔰𝔢(3) relates translational υ and angular ω velocities to “twists” ξ ∈ 𝔰𝔢(3) via

$${\xi }^{\vee }=\left[\begin{array}{c}\hfill \mathit{\upsilon }\hfill \\ \hfill \mathit{\omega }\hfill \end{array}\right],\text{\hspace{1em}and\hspace{1em}}{\left[\begin{array}{c}\hfill \mathit{\upsilon }\hfill \\ \hfill \mathit{\omega }\hfill \end{array}\right]}^{^}=\left[\begin{array}{cc}\hfill \widehat{\mathit{\omega }}\hfill & \hfill \mathit{\upsilon }\hfill \\ \hfill 0\hfill & \hfill 0\hfill \end{array}\right]=\xi .$$

More detail can be found in [12], for example. The identification (20) occurs via left translation, i.e.

$$(g,\stackrel{.}{g})\mapsto (g,g^{-1}\stackrel{.}{g})\mapsto (g,\xi )$$ (21)

where ξ = g⁻¹ ġ, ω = (R⁻¹ Ṙ)^∨, and υ = R⁻¹ ṗ.

B. Fiber Bundles

A bundle [7] is a triple (E, π, M) where E and M are differentiable manifolds and the map π : E → M is a continuous map. The space E is called the entire space, M is called the base space and the map π is called the projection map. The inverse image π⁻¹ (x) is called the fiber over x ∈ M. If all the individual fibers π⁻¹ (x), for x ∈ M are diffeomorphic to the common space F, then F is called the fiber of the bundle and such a bundle is referred to as a fiber bundle. Locally, E = M × F; if this relationship is global, the fiber bundle is trivial. A natural bundle structure arises in the case that a group H, called the symmetry group, acts on the entire space, E, in which case M = E/H can be defined as the quotient space, and H is the fiber.