Home | About | Journals | Submit | Contact Us | Français |

**|**HHS Author Manuscripts**|**PMC2871331

Formats

Article sections

- Abstract
- I. INTRODUCTION
- II. GENERAL FRAMEWORK
- III. FOLLOWING CURVES WITH A PLANAR CART
- IV. APPLICATIONS TO NEEDLE STEERING
- V. TASK-INDUCED REDUCTION ON MECHANICAL SYSTEMS
- VI. DISCUSSION AND FUTURE WORK
- REFERENCES

Authors

Related links

IEEE Trans Automat Contr. Author manuscript; available in PMC 2010 May 17.

Published in final edited form as:

IEEE Trans Automat Contr. 2010; 55(3): 664–673.

doi: 10.1109/TAC.2009.2039241PMCID: PMC2871331

NIHMSID: NIHMS192980

Vinutha Kallem, Department of Mechanical Engineering and Applied Sciences, University of Pennsylvania, Philadelphia, PA 19104, USA (Email: ude.nnepu.saes@mellakv).

Corresponding author: V. Kallem (Email: ude.nnepu.saes@mellakv)

See other articles in PMC that cite the published article.

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 the 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 also demonstrate how the proposed technique dovetails with Lagrangian reduction.

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.

Reducing the dynamics of a system into smaller subsystems potentially simplifies control, planning or estimation tasks. For example, in [1], [2] reduction of snakeboard dynamics into lower dimensional subsystems enables gait generation for the snakeboard. Often, such dimensional reduction is obtained by exploiting intrinsic symmetries in the mechanical system at hand, e.g. invariance of the system’s Lagrangian to some group of motions. Here, we use symmetries that arise from an extrinsic control task, rather than the intrinsic structure of the configuration space, constraints, or system dynamics. We show that such a “task-induced” symmetry can result in a reduction in a class of kinematic and mechanical systems.

Previous work on reduction methods for mechanical systems deals with intrinsic symmetry in a system. If the Lagrangian of a mechanical system is invariant under 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 [3] 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 [5] 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). A general and comprehensive theory of Lagrangian reduction is studied in [6]. In the context of the present work, the symmetry group (or fiber) for a mechanical reduction is conceived as a *principal bundle* in its own right. To elucidate this connection, we compute a reduced system of equations for a differential drive mobile robot, in which we first apply Lagrangian reduction and then apply task-induced reduction.

Hanssmann *et al.* [7] perform reduction to align coordinated rigid bodies relative to one another. This alignment task induces symmetry, affording a *task-induced* reduction of the coupled system dynamics. This reduction is harnessed to developed control laws that align two rigid bodies based on the relative configuration between the bodies. While the authors take a Hamiltonian approach, similar results can be obtained using the two-stage approach—Lagrangian reduction plus task-induced reduction—described in Section V.

The work we present in this paper relates conceptually to the literature on relative equilibria [8]–[11]. Roughly speaking, given a mechanical system with symmetry, a relative equilibrium is a trajectory that is an equilibrium point 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; the tasks we introduce *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. Moreover, task-induced reduction applies to both kinematic and mechanical systems, whereas relative equilibria typically arise in mechanical systems with intrinsic symmetries.

In Section II, we present a general framework to perform task-induced reduction on kinematic systems. In Sections III and IV we apply this technique to a planar cart and to needle steering. We show how task-induced reduction simplifies the control design to achieve the tasks at hand. This culminates in a novel approach for the control of steerable needles, which was the principal motivation for us to study this problem. We then extend this technique in Section V to a class of Lagrangian systems. We use circle following of a differential drive robot as an example to illustrate the task-induced reduction in mechanical systems.

Let *G* be a Lie group.^{1} Consider the left-invariant kinematic system

$${g}^{-1}\dot{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

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

(2)

One way to achieve the task is to design a (feedback) control policy 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* := *G/H* 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 *principal bundle*. Since *H* is a subgroup of *G* there exists a bundle projection π : *G* → *B* = *G/H*. Furthermore, 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*.

*Proposition 2.1:* The vector field on the reduced space, *B*, can be calculated as

$$\dot{r}={f}_{0}(r)+{\displaystyle \sum _{i=1}^{k}{f}_{i}(r){u}_{i}}$$

(3)

where *f _{i}*(

$${h}^{-1}\dot{h}={\text{Ad}}_{s(r)}\phantom{\rule{thinmathspace}{0ex}}\left({\xi}_{0}+{\displaystyle \sum _{i=1}^{k}{\xi}_{i}{u}_{i}}-s{(r)}^{-1}\mathit{\text{Ts}}\xb7\dot{r}\right)\phantom{\rule{thinmathspace}{0ex}}.$$

(4)

*Proof:* Consider a local section *s* : *U* *B* → *G*. By definition, a local section *s* satisfies π *s* = Id_{U}.^{2} This section, *s*, induces the local trivialization which is a local diffeomorphism defined by the map:

$$\begin{array}{cc}\psi :H\phantom{\rule{thinmathspace}{0ex}}\times \phantom{\rule{thinmathspace}{0ex}}U\to {\pi}^{-1}(U),\hfill & (h,r)\to h\phantom{\rule{thinmathspace}{0ex}}s(r).\hfill \end{array}$$

Its inverse ψ^{−1} : π^{−1}(*U*) → *H* × *U* is given by ψ^{−1}(*g*) = (*g* (*s*(π(*g*)))^{−1}, π(*g*)).

To obtain an expression for , differentiate *r* = π(*g*) to obtain

$$\dot{r}={T}_{g}\pi \xb7g{\xi}_{0}+{\displaystyle \sum _{i=1}^{k}{T}_{g}\pi \xb7g{\xi}_{i}{u}_{i}.}$$

In order to show that the is a function of only *r* and *u _{i}*, we exploit the fact that the projection map is invariant to the left action of

$$\begin{array}{cc}\dot{r}(t)\hfill & {=\frac{d}{d\tau}\pi \phantom{\rule{thinmathspace}{0ex}}(g(t+\tau ))\phantom{\rule{thinmathspace}{0ex}}|}_{\tau =0}\hfill \\ \hfill & {=\frac{d}{d\tau}\pi \phantom{\rule{thinmathspace}{0ex}}({h}^{-1}(t)g(t+\tau ))\phantom{\rule{thinmathspace}{0ex}}|}_{\tau =0}\hfill \\ \hfill & ={T}_{{h}^{-1}g}\pi \xb7{h}^{-1}(t)\dot{g}(t)\hfill \\ \hfill & ={T}_{{h}^{-1}g}\pi \xb7{h}^{-1}g\phantom{\rule{thinmathspace}{0ex}}\left({\xi}_{0}+{\displaystyle \sum _{i=1}^{k}{\xi}_{i}{u}_{i}}\right)\hfill \\ \hfill & ={T}_{s(r)}\pi \xb7s(r){\xi}_{0}+{\displaystyle \sum _{i=1}^{k}{T}_{s(r)}\pi \xb7s(r){\xi}_{i}{u}_{i}}.\hfill \end{array}$$

From this equation it is clear that $\dot{r}={f}_{0}(r)+{\displaystyle {\sum}_{i=1}^{k}{f}_{i}(r){u}_{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.^{3} From the definition of ψ we can rewrite *g*^{−1} *ġ* as

$$\begin{array}{cc}{g}^{-1}\dot{g}\hfill & ={(h\phantom{\rule{thinmathspace}{0ex}}s(r))}^{-1}\frac{d}{\mathit{\text{dt}}}(h\phantom{\rule{thinmathspace}{0ex}}s(r))\hfill \\ \hfill & ={\text{Ad}}_{s(r)}^{-1}\phantom{\rule{thinmathspace}{0ex}}{h}^{-1}\dot{h}+s{(r)}^{-1}\mathit{\text{Ts}}\xb7\dot{r}.\hfill \end{array}$$

This results in

$${h}^{-1}\dot{h}={\text{Ad}}_{s(r)}\phantom{\rule{thinmathspace}{0ex}}\left({\xi}_{0}+{\displaystyle \sum _{i=1}^{k}{\xi}_{i}{u}_{i}-s{(r)}^{-1}\mathit{\text{Ts}}\xb7\dot{r}}\right)$$

which also depends only on *r* and *u*. The equation for the fiber variable *h* in (4) is sometimes called the reconstruction equation [13].

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 stay with in a plane or a sphere in ^{3}. Finally, we describe how the task-induced reduction presented in this paper dovetails with Lagrangian reduction for mechanical systems with intrinsic symmetries [4], [5].

In this section we apply “task-induced” reduction to two planar cart examples (Figure 1(A)) as it provides us with a simple system to illustrate the “task-induced” symmetry and reduction technique presented above. In the first example, we apply the technique to line-following of the cart. 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.

Planar cart. (A) Planar cart moving on a 2*D* horizontal plane. The *x*-axis of the body frame points along the forward velocity of the cart. (B) Cart following a line. Red dotted line is the desired line to be followed by the cart. Initially the cart starts **...**

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)^{2} *S*^{1}^{2}. To denote an element *g* SE(2), we use both *g* = (*R, p*) SO(2)^{2} and its homogeneous representation

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

(5)

The kinematic equation of the unicycle can be written as

$${g}^{-1}\dot{g}=\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{cc}\hfill \widehat{\mathrm{\Omega}}\hfill & \hfill \left[\begin{array}{c}\upsilon \\ 0\end{array}\right]\hfill \\ \hfill 0\hfill & \hfill 0\hfill \end{array}\right]\phantom{\rule{thinmathspace}{0ex}},\phantom{\rule{thinmathspace}{0ex}}\text{where}\phantom{\rule{thinmathspace}{0ex}}\widehat{\mathrm{\Omega}}=\left[\begin{array}{cc}\hfill 0\hfill & \hfill -\omega \hfill \\ \hfill \omega \hfill & \hfill 0\hfill \end{array}\right]\phantom{\rule{thinmathspace}{0ex}}.$$

(6)

We now consider two control tasks, following a line and following a circle.

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

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 (hg)=\phi (g)\}\hfill \\ \hfill & =\left\{h\in \text{SE}(2)\phantom{\rule{thinmathspace}{0ex}}|\phantom{\rule{thinmathspace}{0ex}}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]\phantom{\rule{thinmathspace}{0ex}},x\in \mathbb{R}\right\}\cong \mathbb{R}.\hfill \end{array}$$

Let *B* := *G/H* *S*^{1} × . 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\phantom{\rule{thinmathspace}{0ex}},p+{\mathit{\text{xe}}}_{1})=({R}^{-1}{e}_{1},{e}_{2}^{T}p)=\pi (g)$. Therefore, π is *H* invariant. Also, if *g*_{1} π^{−1}(*r*) then *g*_{2} = *hg*_{1} π^{−1}(*r*), ∀*h* *H*. Therefore π^{−1}(*r*) *H* for each *r* *G/H*. Let *r* = [*r*_{1}, *r*_{2}, *r*_{3}]^{T} ^{3} denote an element in *B* with ${r}_{1}^{2}+{r}_{2}^{2}=1$. Define a *global* section *s* : *B* = *S*^{1} × → SE(2) by

$$s(r)=\left[\begin{array}{ccc}{r}_{1}\hfill & {r}_{2}\hfill & 0\hfill \\ -{r}_{2}\hfill & {r}_{1}\hfill & {r}_{3}\hfill \\ 0\hfill & 0\hfill & 1\hfill \end{array}\right]\phantom{\rule{thinmathspace}{0ex}}.$$

Note that π *s* = Id_{B}. Hence, the projection π : SE(2) → *S*^{1} × defines a trivial bundle. With this section, a global diffeomorphism ψ : *H* × *B* × (*S*^{1} × ) → SE(2) can be constructed by ψ (*h, r*) = *hs*(*r*). This principal bundle can be represented by the following diagram:

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

$$\dot{r}=\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{c}0\\ 0\\ -{r}_{2}\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}+\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{c}{r}_{2}\\ -{r}_{1}\\ 0\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}\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

$$\dot{x}={r}_{1}.$$

(8)

Observe that *R*^{−1}*e*_{1} is the unit vector along the *x*-axis of the world reference frame as 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}\phantom{\rule{thinmathspace}{0ex}}\text{for}\phantom{\rule{thinmathspace}{0ex}}\text{some}\phantom{\rule{thinmathspace}{0ex}}k\in {\mathbb{R}}^{+}.$$

(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 $\dot{\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*^{1} × : *r*_{2} = 0} Ω_{c}. The largest invariant subset in *E* contains only the points *r* = [±1, 0, 0]^{T}. Therefore from LaSalle’s invariance principle, we conclude that the cart follows the desired line. Further by considering the local chart (*r*_{2}, *r*_{3}) and by eigenvalue analysis we can see that the equilibrium point *r* = [1, 0, 0]^{T} is stable and the other one is a saddle. The additional (unstable) critical point is a topological obstruction: our essentially global controller is the most that can be expected of a smooth feedback on *S*^{1} × . To follow the line in the other direction, simply let ω = *r*_{3} − *kr*_{2} for some *k* ^{+}.

In Figure 1(B) we show an anecdotal trajectory (in simulation) the cart follows using the above developed controller. As we can see from the figure, the cart is initially far away from the *x*-axis and it asymptotically follows the desired line.

Suppose we are interested in making the cart follow a circle given by *X* = {(*x, y*) ^{2} | *x*^{2} + *y*^{2} − ρ^{2} = 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) with

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

Notice that *B* := SE(2)/SO(2) ^{2}. If we introduce a global section *s* : ^{2} → SE(2) given by

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

for *r* , then we can see that π defines a trivial bundle. This SO(2)-bundle can be summarized by the following diagram:

Let *r* = [*r*_{1}, *r*_{2}]^{T}. Assuming that the forward velocity of the cart is constant (say, υ = 1), the reduced space dynamics are given by

$$\dot{r}={R}^{-1}\dot{p}-{R}^{-1}\dot{R}r=\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{c}1\\ 0\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}+\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}r\omega $$

(10)

and the fiber variable θ satisfies

$$\dot{\theta}=\omega .$$

(11)

Observe that *r* = *R*^{−1}*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 then *r* = [0, ρ]^{T}, and if it is following the desired circle counter-clockwise then *r* = [0, −ρ]^{T}. 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},\phantom{\rule{thinmathspace}{0ex}}\text{for}\phantom{\rule{thinmathspace}{0ex}}\text{some}\phantom{\rule{thinmathspace}{0ex}}k\in {\mathbb{R}}^{+}.$$

(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 $\dot{\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. To follow the circle counter-clockwise, set $\omega =\frac{1+\mathit{\text{rk}}}{\rho}$. Note that the convergence is global.

Figure 1(C) shows an example simulation of the circle following by a cart. The cart starts away from the desired circle and asymptotically follows the desired circle.

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 [14], [15]. 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 generalization of the nonholonomic bicycle, and torsional compliance of the needle shaft is neglected (Figure 2(A)). In this model, _{1} and _{2} 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=\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{cc}R\hfill & p\hfill \\ {0}^{T}\hfill & 1\hfill \end{array}\right]\in \text{SE}(3)\phantom{\rule{thinmathspace}{0ex}}\text{where}\phantom{\rule{thinmathspace}{0ex}}R\in \text{SO}(3)\phantom{\rule{thinmathspace}{0ex}}\text{and}\phantom{\rule{thinmathspace}{0ex}}p\in {\mathbb{R}}^{3}.$$

(13)

Webster *et al.* obtain a kinematic model for needle steering,

$${g}^{-1}\dot{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}={[\begin{array}{cccccc}0& 0& 1& \kappa & 0& 0\end{array}]}^{T}$ and ${\xi}_{2}^{\vee}={[\begin{array}{cccccc}0& 0& 0& 0& 0& 1\end{array}]}^{T}$.

Ideally when 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 ϕ/_{1}.

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 cannot necessarily be tracked asymptotically. In fact, 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 that cannot be driven back to the desired trajectory in an arbitrarily small amount of time. To see this, refer to Figure 2(B).

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.* [16], [17]. Their planners, which rely 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 prior work, Kallem and Cowan [18] 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 re-planning and low-level control architecture. We envision a “subspace planner” that chooses feasible subspaces, upon which an Alterovitz-style planner [16] builds paths within each subspace.

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.

To the best of our knowledge, prior work by Kallem and Cowan [18], [19] is the only previous work done on controlling the flexible bevel-tip needle. In this work, the authors use 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.

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 as the zero of the function, given by

$$\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

Here 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\in \text{SE}(3)\phantom{\rule{thinmathspace}{0ex}}|\phantom{\rule{thinmathspace}{0ex}}x=({e}^{{\widehat{e}}_{1}\theta},p)\phantom{\rule{thinmathspace}{0ex}}\text{where}\phantom{\rule{thinmathspace}{0ex}}\theta \in \mathbb{R}\phantom{\rule{thinmathspace}{0ex}}\text{and}\phantom{\rule{thinmathspace}{0ex}}p\in {\mathbb{R}}^{3}\}\hfill \\ \hfill & =X\cong \text{SO}(2)\u24c8{\mathbb{R}}^{3}\cong \text{SE}(2)\phantom{\rule{thinmathspace}{0ex}}\times \phantom{\rule{thinmathspace}{0ex}}\mathbb{R}.\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*^{2}. 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*^{2} − {(−1, 0, 0)}. Define the bundle section on *U, s* : *U* → π^{−1}(*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 3. 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*^{2}, consider *U*′ = (*S*^{2} − {(1, 0, 0)}) and a similar section can be constructed.^{5} These make the map π a principal bundle which can be summarized by the following commutative diagram:

Illustration of the bundle section. Unit vector bisects *r* and *e*_{1}. If the *r* is rotated about as axis by π then it coincides with *e*_{1}. The circle through the points *r* and (1, 0, 0) is a latitude of *S*^{2} drawn perpendicular to . **...**

We assume that the insertion speed is held constant at υ = 1. Base space elements can be written in coordinates as *r* = [*r*_{1}, *r*_{2}, *r*_{3}]^{T} ^{3} with ${r}_{1}^{2}+{r}_{2}^{2}+{r}_{3}^{2}=1$. The base space dynamics are

$$\dot{r}=\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{c}\hfill 0\hfill \\ \hfill {\kappa r}_{3}\hfill \\ \hfill -{\kappa r}_{2}\hfill \end{array}\right]\phantom{\rule{thinmathspace}{0ex}}+\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{c}\hfill {r}_{2}\hfill \\ \hfill -{r}_{1}\hfill \\ \hfill 0\hfill \end{array}\right]\phantom{\rule{thinmathspace}{0ex}}\omega .$$

(14)

The fiber variable *h* = (*e*^{ê1θ}, *p*) *H* satisfies

$$\begin{array}{cc}\dot{\theta}{\widehat{e}}_{1}\hfill & ={\text{Ad}}_{\overline{s}(r)}\phantom{\rule{thinmathspace}{0ex}}\left(\kappa {\widehat{e}}_{1}+\omega {\widehat{e}}_{3}-\overline{s}{(r)}^{-1}\frac{\partial \overline{s}(r)}{\partial r}\dot{r}\right)\phantom{\rule{thinmathspace}{0ex}},\hfill \\ \dot{p}\hfill & ={e}^{{\widehat{e}}_{1}\theta}\overline{s}(r){e}_{3},\hfill \end{array}$$

(15)

where (*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},\phantom{\rule{thinmathspace}{0ex}}\text{for}\phantom{\rule{thinmathspace}{0ex}}\text{some}\phantom{\rule{thinmathspace}{0ex}}k\in {\mathbb{R}}^{+}.$$

(16)

Once again, we use the task function in base coordinates as a candidate Lyapunov function, namely (*r*) = 1−*r*_{1}. The time derivative of the Lyapunov function is $\dot{\phi}(r)=-{\mathit{\text{kr}}}_{2}^{2}$ which is negative semi-definite in *r*. The set where = 0 is given by *E* = {*r* = [*r*_{1}, *r*_{2}, *r*_{3}]^{T} *S*^{2}|*r*_{2} = 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 the needle tip can be stabilized to a plane parallel to the desired plane. Notice that *asymptotically* the fiber dynamics become

$$\dot{\theta}=\kappa ;\phantom{\rule{thinmathspace}{0ex}}\dot{p}={(0,\text{sin}\phantom{\rule{thinmathspace}{0ex}}\theta ,-\text{cos}\phantom{\rule{thinmathspace}{0ex}}\theta )}^{T}.$$

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*_{2}, *r*_{3}) and by eigen-value analysis one can show 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*_{2} for some *k* ^{+}.

Figure 4(A) shows an example simulation run. The initial condition is such that without any control the needle tip would trace circle not parallel to the *y – z* plane. With the use of the above developed controller, the needle tip in steady state traces a circle parallel to the desired *y – z* plane.

In this section we use the reduction technique developed in section 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 such a circle on the surface of the sphere, the position vector of the center of the sphere written in the body frame is given by *R*^{−1}*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* = ^{3}. The bundle projection π : SE(3) → ^{3} is given by

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

where *g* = (*R, p*) SO(3)^{3} SE(3). This bundle is trivial because we can construct a global section *s* : ^{3} → SE(3) as

$$s(r)=\left[\begin{array}{cc}{I}_{3\times 3}& r\\ {0}^{T}& 1\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}.$$

Given (*R, r*) *H* × *B* = SO(3) × ^{3}, the corresponding *g* SE(3) can be calculated as $g=\left[\begin{array}{cc}R& \mathit{\text{Rr}}\\ 0& 1\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}$. This principal bundle can be represented by the following commutative diagram:

Assuming υ = 1, the dynamics in the base space can be written as

$$\dot{r}=\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{c}0\\ {\kappa r}_{3}\\ 1-{\kappa r}_{2}\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}+\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{c}{r}_{2}\\ -{r}_{1}\\ 0\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}\omega $$

(17)

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

$${R}^{-1}\dot{R}=\kappa {\widehat{e}}_{1}+{\widehat{e}}_{3}\omega .$$

(18)

The control input

$$\omega =-k({\mathit{\text{dr}}}_{2}+\varrho {r}_{1})\phantom{\rule{thinmathspace}{0ex}}\text{for}\phantom{\rule{thinmathspace}{0ex}}\text{some}\phantom{\rule{thinmathspace}{0ex}}k\in {\mathbb{R}}^{+}$$

(19)

makes the base variable, *r*, converge to [−*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 derivative, (*r*) = ω(*dr*_{2} + *r*_{1}) = −(*dr*_{2} + *r*_{1})^{2}, 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*_{2} − *r*_{1}.

Figure 4(B) is an anecdotal simulation of the needle staying on the surface of a desired sphere. The needle starts away from the desired sphere and asymptotically converges to the surface of the sphere.

In this section we present how the above developed task-induced reduction extends to the Lagrangian systems. In the first stage, we perform a previously developed mechanics-based “intrinsic” reduction to separate the configuration space into a shape space and a symmetry (Lie) group. We then perform task-induced reduction to further reduce the symmetry group into a base space and a fiber as in Section II.

Consider the mechanical system whose configuration space is *Q*, Lagrangian is *L*(*q, *), (*q*, ) *TQ* and has only nonholonomic constraints of the form *w*(*q*) = 0. Assume that the Lagrangian and the constraints are invariant to a left action of a Lie group, *G*. It has been previously shown that such a system can be decoupled into the symmetry group, *G* and the shape space, *S* = *Q/G*,^{6} namely *Q* = *G* × *S* (locally). The reduced equations are given in the following Proposition.

*Proposition 5.1:* (Ostrowski [5]) For the above described mechanical system, assume that the input forcing occurs only in the shape space and that no constraint lies completely in the shape space. The reduced system equations can be written as

$$M(\sigma )\ddot{\sigma}=-C(\sigma ,\dot{\sigma})\dot{\sigma}-N(\sigma ,\dot{\sigma},\Pi )+\tau $$

(20)

$$\dot{\Pi}=f(\sigma ,\dot{\sigma},\Pi )\phantom{\rule{thinmathspace}{0ex}},$$

(21)

where σ *S* denotes the shape variable, Π * denotes the generalized momenta and τ the generalized input force on the systems. The symmetry group variable, *g* *G*, can be reconstructed as

$${g}^{-1}\dot{g}=-\mathbb{A}(\sigma )\dot{\sigma}+{\overline{I}}^{-1}(\sigma )\Pi .$$

(22)

If there are *m* nonholonomic constraints, then there are *n* − *m* generalized momenta, where *n* is the dimension of the Lie group, *G*. In particular, for principally kinematic systems *m* = *n* thus resulting in no generalized momenta. In other words, for these systems the group variable can be reconstructed from the nonholonomic constraint equations and this simplifies the reduced system equations further. More details on the structure and computations can be found in propositions 4.3 and 5.1 in [5] and also in [4], [8].

Observe that the symmetry group dynamics given in (22) is a left-invariant vector field on the Lie group, *G*. Assume that there is a task defined completely on the group space. If this task induces a symmetry as described in previous sections, we can further decompose *G*. As before, 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)\}.$$

Since *H* is a subgroup of *G*, there exists a bundle projection π : *G* → *B* := *G/H*. Also since *H* does not affect the control task, we consider the state evolution only in the base space.

*Corollary 5.2:* The reduced equations in the base space, *B* = *G/H*, are

$$\dot{r}={f}_{1}(r,\sigma ,\Pi )+{f}_{2}(r,\sigma )\dot{\sigma}$$

(23)

where *f*_{1}(*r*, σ, *p*) = *T*π · *g**Ī*^{−1}*p* and *f*_{2}(*r*, σ) = −*T*π · *g*(σ) and those in the fiber, *H*, are

$${h}^{-1}\dot{h}={\text{Ad}}_{s(r)}\phantom{\rule{thinmathspace}{0ex}}(-\mathbb{A}(\sigma )\dot{\sigma}+{\overline{I}}^{-1}(\sigma )\Pi -s{(r)}^{-1}\mathit{\text{Ts}}\xb7\dot{r})\phantom{\rule{thinmathspace}{0ex}}.$$

(24)

*Proof:* Following the proof for the proposition 2.1, the base dynamics (23) result from the observation that the vector field on *G* is left-invariant to *G* and the projection π is invariant to *H*, which is a subgroup of *G*. The fiber dynamics (24) is exactly the same computation as (4).

Locally, Proposition 5.1 decomposes an entire configuration space as the cross product of the symmetry group and shape space, namely *Q* = *G* × *S* *G* × (*Q/G*). Corollary 5.2 can be conceived as further decomposing *G* into a base and fiber in its own right, due to the task-induced symmetry fiber *H*, so that we have

$$Q=\stackrel{G}{\overbrace{H\phantom{\rule{thinmathspace}{0ex}}\times \phantom{\rule{thinmathspace}{0ex}}\stackrel{B}{\overbrace{(G/H)}}}}\phantom{\rule{thinmathspace}{0ex}}\times \phantom{\rule{thinmathspace}{0ex}}\stackrel{S}{\overbrace{(Q/G)}}$$

(25)

at least locally, or globally in the case that *G* = *H* × *B* and *Q* = *G* × *S* are both trivial bundles. Here, *Q* is decomposed via the mechanical reduction of Proposition 5.1, and *G* is further decomposed via the task reduction of Corollary 5.2.

We illustrate task-induced reduction on mechanical systems via an example: circle-following of a differential drive robot moving on a 2D horizontal plane as shown in Figure 5(A). Let *g* SE(2) be the position and orientation of the differential drive robot and (σ_{1}; σ_{2}) be the wheel angles. Let ρ_{w} be the wheel radius and *w* be the half-distance between the wheel axes, and *m, J* and *J _{w}* be the mass of the robot, inertia of the robot and inertia of the wheels respectively, and τ = [τ

$$M\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{c}{\ddot{\sigma}}_{1}\\ {\ddot{\sigma}}_{2}\end{array}\right]=\left[\begin{array}{c}{\tau}_{1}\\ {\tau}_{2}\end{array}\right]\phantom{\rule{thinmathspace}{0ex}},$$

(26)

where

$$M=\left[\begin{array}{cc}{J}_{w}+\frac{m{\rho}_{w}^{2}}{4}+\frac{J{\rho}_{w}^{2}}{4{w}^{2}}& \frac{m{\rho}_{w}^{2}}{4}-\frac{J{\rho}_{w}^{2}}{4{w}^{2}}\\ \frac{m{\rho}_{w}^{2}}{4}-\frac{J{\rho}_{w}^{2}}{4{w}^{2}}& {J}_{w}+\frac{m{\rho}_{w}^{2}}{4}+\frac{J{\rho}_{w}^{2}}{4{w}^{2}}\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}.$$

The symmetry group dynamics are

$${g}^{-1}\dot{g}=\left[\begin{array}{ccc}\hfill 0\hfill & \hfill -{\dot{\varsigma}}_{2}\hfill & \hfill {\dot{\varsigma}}_{1}\hfill \\ \hfill {\dot{\varsigma}}_{2}\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \end{array}\right]\phantom{\rule{thinmathspace}{0ex}},$$

(27)

where

$${\varsigma}_{1}=\frac{\rho w}{2}({\sigma}_{1}+{\sigma}_{2})\phantom{\rule{thinmathspace}{0ex}}\text{and}\phantom{\rule{thinmathspace}{0ex}}{\varsigma}_{2}=\frac{\rho w}{2w}({\sigma}_{1}-{\sigma}_{2})$$

is a reparameterization of the shape space, ς̇_{1} is the forward velocity and ς̇_{2} is the angular velocity of the robot. Also, let ${u}_{1}=\frac{\rho w}{2}{({e}_{1}+{e}_{2})}^{T}{M}^{-1}\tau $ and ${u}_{2}=\frac{\rho w}{2w}{({e}_{1}-{e}_{2})}^{T}{M}^{-1}\tau $, where *e*_{1} = [1, 0]^{T} and *e*_{2} = [0, 1]^{T}. In these new coordinates, the shape space dynamics are given by

$$\left[\begin{array}{c}{\ddot{\varsigma}}_{1}\\ {\ddot{\varsigma}}_{2}\end{array}\right]=\left[\begin{array}{c}{u}_{1}\\ {u}_{2}\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}.$$

(28)

(A) Differential drive robot. World and body reference frames are shown. The body frame’s *x*-axis is along the forward translational velocity of the robot. (B) Differential drive robot following a circle. Red dotted circle is the desired circle **...**

Consider the task of making the differential drive robot follow a circle of radius ρ whose center is at the origin of the world frame. Observe that the symmetry group dynamics resemble the equations of motion of a planar cart with the forward and angular velocities of the cart given as functions of the wheel velocities. As in section III-B, we perform a task-induced reduction with SO(2) as the fiber. The task-induced reduced space is given by *r* = *R*^{−1}*p*, where *g* = (*R, p*). The dynamics of the reduced space is given by

$$\dot{r}=\left[\begin{array}{c}1\\ 0\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}{\dot{\varsigma}}_{1}+\phantom{\rule{thinmathspace}{0ex}}\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}r{\dot{\varsigma}}_{2}$$

(29)

and the fiber variable, θ, satisfies

$$\dot{\theta}={\dot{\varsigma}}_{2}.$$

(30)

We now use the control scheme developed in Section III-B along with an integrator back-stepping algorithm [20] to make the differential drive robot follow the desired circle. One can show that the following control law makes the robot follow the desired circle

$$u=\frac{\partial \varphi}{\partial r}A\dot{\varsigma}-{\left(\frac{\partial \phi}{\partial r}A\right)}^{T}-k(\dot{\varsigma}-\varphi ),\text{\hspace{1em}}k\in {\mathbb{R}}^{+},$$

where

$$\begin{array}{cc}A=\left[\begin{array}{cc}\hfill 1\hfill & \hfill {r}_{2}\hfill \\ \hfill 0\hfill & \hfill -{r}_{1}\hfill \end{array}\right]\phantom{\rule{thinmathspace}{0ex}},& \varphi =\left[\begin{array}{c}1\\ -(1+{r}_{1})/\rho \end{array}\right]\end{array}\phantom{\rule{thinmathspace}{0ex}},$$

and

$$\phi =\frac{1}{2}{r}_{1}^{2}+\frac{1}{2}{({r}_{2}-\rho )}^{2}.$$

Global convergence of this controller can be shown using the Lyapunov function, $V=\phi +\frac{1}{2}{\Vert \dot{\varsigma}-\varphi \Vert}^{2}$. Figure 5(B) is an example simulation of the circular wall-following in a differential drive robot. The robot is initially away from the desired circle but, it asymptotically follows the desired circle.

We develop a general framework to perform task-specific reduction in kinematic systems on Lie groups. We apply this method to 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 both the examples, task-induced reduction enables us to design essentially global feedback controllers on the reduced systems. We envision that the methodology presented in this paper will form the basis for a new planning and control framework for needle steering.

We extend the framework to mechanical systems whose Lagrangian and constraints are left-invariant to a Lie group. From previous work on “intrinsic” Lagrangian reduction it is known that dynamics on the shape space can be obtained from the reduced Lagrangian, while a reconstruction equation generates motions in the symmetry group. This reconstruction equation describes how motions in the shape space produce Lie group motions. As we show in the differential drive robot example, certain control tasks enable us to perform a second “extrinsic” (task-specific) reduction, thus further decomposing the symmetry group into a base and fiber. Such an analysis is also amenable to other mechanical systems like skateboards, bicycles, and snake robots. As in the needle steering case, task-induced symmetry will likely facilitate planning and control for other mechanical systems.

In all the examples in Sections III and IV, task-induced reduction simplifies control design. Further, in the kinematic systems, the task-functional serves as a Lyapunov function and the control input is chosen accordingly. In the differential drive robot example, the task-functional is again used as a part of the Lyapunov function. In all these cases, the task-functional provides a guiding mechanism for controller design. This suggests that there may be a general control theory for systems exhibiting task-induced symmetry, at least for some class of mechanical systems. Specifically, we expect this to hold for principally kinematic systems whose base variables are completely controllable. For these systems, we speculate that integrator back-stepping is possible, thus reducing the mechanical control problem to that of kinematic control. This direction of future research, we think, will be very useful in controlling complicated higher-dimensional systems.

The authors thank Ron Alterovitz for stimulating discussions on integration of planning and control in needle steering and Eatai Roth for help with illustrations.

This work was supported by the National Institutes of Health under grants R21-EB00345 and R01-EB006435.

**Vinutha Kallem** received the B.Tech. degree from Indian Institute of Technology, Madras in 2002 and the M.S. degree from Stanford University in 2004 and the Ph.D. degree from Johns Hopkins University in 2008; all in Mechanical Engineering. She is currently a Postdoctoral Fellow in the Department of Mechanical Engineering and Applied Mechanics at University of Pennsylvania.

**Dong Eui Chang** received the B.S. degree in control and instrumentation engineering and the M.S. degree in electrical engineering, both from Seoul National University, Seoul, Korea, in 1994 and 1997, respectively, and the Ph.D. degree in control and dynamical systems from the California Institute of Technology, Pasadena, in 2002. He is currently an Assistant Professor with the Department of Applied Mathematics, University of Waterloo, Waterloo, Ontario, Canada.

**Noah J. Cowan** (S’98, M’01) received the B.S. degree in electrical engineering from the Ohio State University, Columbus, in 1995, and the M.S. and Ph.D. degrees in electrical engineering and computer science from the University of Michigan, Ann Arbor, in 1997 and 2001.

From 2001 to 2003, he was a Postdoctoral Fellow at the University of California, Berkeley. In 2003, he joined the faculty of the Johns Hopkins University, Baltimore, MD, as an Assistant Professor. Prof. Cowan’s research interests include multisensory control in animals and machines.

Prof. Cowan received the NSF CAREER award in 2009, the William H. Huggins Award for excellence in teaching in 2004, and a Rackham Doctoral Fellowship from the University of Michigan in 2000.

The Lie groups discussed in this paper in detail are the Euclidean groups SE(2) = SO(2)^{2} and SE(3) = SO(3)^{3}. The symbol denotes semi-direct product. If *R*_{1}, *R*_{2} SO(*n*) and *p*_{1}, *p*_{2} ^{n}, the product in the group obtained by the semi-direct product of SO(*n*) and ^{n} is given by (*R*_{1}, *p*_{1})*(*R*_{2}, *p*_{2}) = (*R*_{1} *R*_{2}, *p*_{1} + *R*_{1}*p*_{2}). Note that as sets, SE(*n*) = SO(*n*) × ^{n} but as groups, SE(*n*) = SO(*n*)^{n}. We identify the tangent space *T*SE(3) of the Lie group SE(*n*) with

$$T\text{SE}(n)\simeq \text{SE}(n)\phantom{\rule{thinmathspace}{0ex}}\times \phantom{\rule{thinmathspace}{0ex}}\U0001d530\U0001d522(n)\phantom{\rule{thinmathspace}{0ex}},$$

(31)

where (*n*) is the Lie algebra of SE(*n*). More details may be found in [21], for example.

In particular for *n* = 3, the isomorphism ^{3} (3) is defined using the “wedge/hat” operators by

$${^}_{:}\left[\begin{array}{c}{\omega}_{1}\\ {\omega}_{2}\\ {\omega}_{3}\end{array}\right]\leftrightarrow \left[\begin{array}{ccc}0& -{\omega}_{3}& {\omega}_{2}\\ {\omega}_{3}& 0& -{\omega}_{1}\\ -{\omega}_{2}& {\omega}_{1}& 0\end{array}\right]{:}^{\vee}$$

where (3) is the Lie algebra of SO(3). In a standard abuse of notation the wedge/hat isomorphism ^{3}^{3} (3) relates translational υ and angular ω velocities to “twists” ξ (3) via

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

The identification (31) can be rewritten as

$$(g,\dot{g})\mapsto (g,{g}^{-1}\dot{g})\mapsto (g,\xi )$$

(32)

where ξ = *g*^{−1} *ġ*, ω = (*R*^{−1} )^{}, and υ = *R*^{−1}.

^{1}We assume the reader is familiar with notions from Lie group theory and principal bundles as it relates to reduction. The appendix reviews some basic facts about the Lie groups SE(2) and SE(3). For more detail, see [8], [12].

^{2}For all *r* *U*, π(*s*(*r*)) = *r*.

^{3}In the examples that follow, the local chart covers all but a set of measure zero of the base space.

^{4}We assume that υ = 1 for ease of presentation of the controller without any loss of generality. This assumption is equivalent to choosing the control input as

$$\left[\begin{array}{c}\upsilon \\ \omega \end{array}\right]=\left[\begin{array}{c}1\\ {\mathit{\text{kr}}}_{2}-{r}_{3}\end{array}\right]\phantom{\rule{thinmathspace}{0ex}}\text{for}\phantom{\rule{thinmathspace}{0ex}}\text{some}\phantom{\rule{thinmathspace}{0ex}}k\in {\mathbb{R}}^{+}.$$

^{5}One can choose *s′* (*r*) = (*e*^{ê3π}*e*^{′π}, 0), where $\varpi \prime =\frac{-{e}_{1}+r}{\Vert -{e}_{1}+r\Vert}$.

^{6}In the literature the space *S* = *Q/G* is referred as both shape space and as base space. In the current work, we only refer to it only as the shape space and *B* = *G/H* (defined in Section II) as the base space.

Portions of this work were previously presented at Intelligent Robots and Systems (IROS), San Diego, CA, October 30–November 1, 2007.

Vinutha Kallem, Department of Mechanical Engineering and Applied Sciences, University of Pennsylvania, Philadelphia, PA 19104, USA (Email: ude.nnepu.saes@mellakv).

Dong Eui Chang, Department of Applied Mathematics, University of Waterloo, Waterloo, ON, N2L 3G1, Canada (Email: ac.oolretawu.htam@gnahced).

Noah J. Cowan, Department of Mechanical Engineering, Johns Hopkins University, Baltimore, MD 21218, USA (Email: ude.uhj@nawocn)..

1. Ostrowski JP. Ph.D. dissertation. California Institute of Technology; 1995. The mechanics and control of undulatory robotic locomotion. [Online]. Available: http://www.cis.upenn.edu/~jpo/papers.html.

2. Ostrowski J, Burdick J. The geometric mechanics of undulatory robotic locomotion. Int. J. Robot. Res. 1998

3. Marsden JE, Ostrowski JP. Symmetries in motion: Geometric foundations of motion control. Nonlinear Science Today. 1998

4. Bloch A, Krishnaprasad P, Marsden J, Murray R. Nonholonomic mechanical systems with symmetry. Archive for Rational Mechanics and Analysis. 1996;vol. 136:21–99.

5. Ostrowski J. Computing reduced equations for robotic systems with constraints and symmetries. IEEE Trans. Robot. Autom. 1999;vol. 15(no. 1):111–123.

6. Cendra H, Marsden JE, Raithu T. Lagrangian Reduction by Stages. Mem. Amer. Math. Soc.; 2001.

7. Hanssmann H, Leonard NE, Smith TR. Symmetry and reduction for coordinated rigid bodies. European J. Control. 2006;vol. 2:176–194.

8. Bloch A. Nonholonomic Mechanics and Control. vol. 24. NY: Springer; 2003.

9. Marsden J. Lectures on Mechanics. Cambridge University Press; 1992.

10. Jalnapurkar S, Marsden J. Stabilization of relative equilibria. IEEE Trans. Autom. Control. 2000;vol. 45(no. 8):1483–1491.

11. Bullo F, Lewis AD. Reduction, linearization, and stability of relative equilibria for mechanical systems on Riemannian manifolds. Acta Applicandae Mathematicae. 2007;vol. 1:53–95.

12. Kobayashi S, Nomizu K. Foundations of Differential Geometry. Vol. I. Wiley-Interscience; 1996.

13. Marsden J, Montgomery R, Ratiu T. Reduction, Symmetry and Phases in Mechanics. American Mathematical Society; 1990.

14. Webster RJ, III, Kim JS, Cowan NJ, Chirikjian GS, Okamura AM. Nonholonomic modeling of needle steering. Int. J. Robot. Res. 2006 May;vol. 25(no. 5/6):509–526. [Online]. Available: http://dx.doi.org/10.1177/0278364906065388.

15. Engh J, Podnar G, Khoo S, Riviere C. Flexible needle steering system for percutaneous access to deep zones of the brain; Proc. IEEE Northeast Bioengineering Conf; 2006. Apr., pp. 103–104.

16. Alterovitz R, Goldberg K, Okamura A. Planning for steerable bevel-tip needle insertion through 2D soft tissue with obstacles; Proc. IEEE Int. Conf. Robot. Autom; 2005. pp. 1652–1657.

17. Alterovitz R, Lim A, Goldberg K, Chirikjian GS, Okamura AM. Steering flexible needles under markov motion uncertainty; Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst; IEEE/RSJ; 2005. Aug., pp. 1570–1575.

18. Kallem V, Cowan NJ. Image-guided control of flexible bevel-tip needles; Proc. IEEE Int. Conf. Robot. Autom; Rome, Italy. 2007. Apr., pp. 3015–3020. [Online]. Available: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=4209548. [PMC free article] [PubMed]

19. Kallem V, Cowan NJ. Image guidance of flexible tip-steerable needles. IEEE Trans. Robot. 2009;vol. 25:191–196. [Online]. Available: http://dx.doi.org/10.1109/TRO.2008.2010357. [PMC free article] [PubMed]

20. Khalil HK. Nonlinear Systems. 2nd ed. Prentice Hall; 2002.

21. Marsden JE, Ratiu TS. Introduction to Mechanics and Symmetry. Springer; 1999.