The International Journal of Robotics Research...

http://ijr.sagepub.com/Robotics Research

The International Journal of

http://ijr.sagepub.com/content/31/2/151The online version of this article can be found at:

DOI: 10.1177/0278364911429977

2012 31: 151 originally published online 22 December 2011The International Journal of Robotics ResearchKonstantin M Seiler, Surya PN Singh, Salah Sukkarieh and Hugh Durrant-Whyte

Using Lie group symmetries for fast corrective motion planning

Published by:

http://www.sagepublications.com

On behalf of:

Multimedia Archives

can be found at:The International Journal of Robotics ResearchAdditional services and information for

http://ijr.sagepub.com/cgi/alertsEmail Alerts:

http://ijr.sagepub.com/subscriptionsSubscriptions:

http://www.sagepub.com/journalsReprints.navReprints:

http://www.sagepub.com/journalsPermissions.navPermissions:

http://ijr.sagepub.com/content/31/2/151.refs.htmlCitations:

What is This?

- Dec 22, 2011OnlineFirst Version of Record

- Feb 3, 2012Version of Record >>

at UQ Library on February 12, 2012ijr.sagepub.comDownloaded from

http://ijr.sagepub.com/

http://ijr.sagepub.com/content/31/2/151

http://www.sagepublications.com

http://www.ijrr.org/

http://ijr.sagepub.com/cgi/alerts

http://ijr.sagepub.com/subscriptions

http://www.sagepub.com/journalsReprints.nav

http://www.sagepub.com/journalsPermissions.nav

http://ijr.sagepub.com/content/31/2/151.refs.html

http://ijr.sagepub.com/content/31/2/151.full.pdf

http://ijr.sagepub.com/content/early/2011/12/22/0278364911429977.full.pdf

http://online.sagepub.com/site/sphelp/vorhelp.xhtml


Using Lie group symmetries for fastcorrective motion planning

The International Journal ofRobotics Research31(2) 151–166© The Author(s) 2011Reprints and permission:sagepub.co.uk/journalsPermissions.navDOI: 10.1177/0278364911429977ijr.sagepub.com

Konstantin M Seiler1, Surya PN Singh2, Salah Sukkarieh1 and Hugh Durrant-Whyte3

AbstractIn this paper we develop an algorithmic framework allowing for fast and elegant path correction exploiting Lie groupsymmetries and operating without the need for explicit control strategies such as cross-track regulation. These systemsoccur across the gamut of robotics, notably in locomotion, be it ground, underwater, airborne, or surgical domains. Insteadof reintegrating an entire trajectory, the method selectively alters small key segments of an initial trajectory in a consistentway so as to transform it via symmetry operations. The algorithm is formulated for arbitrary Lie groups and applied inthe context of the special Euclidean group and subgroups thereof. A sampling-based motion planner is developed thatuses this method to create paths for underactuated systems with differential constraints. It is also shown how the pathcorrection method acts as a controller within a feedback control loop for real-time path correction. These approaches aredemonstrated for ground vehicles in the plane and for flexible bevel tip needle steering in space. The results show thatusing symmetry-based path correction for motion planning provides a prudent and simple, yet computationally tractable,integrated planning and control strategy.

KeywordsNonholonomic planning, Lie group symmetry, path correction, online systems, sampling based planning

1. Introduction

Corrective motion planning seeks to efficiently adapt agiven trajectory to suit a new, slightly modified situation.Of particular interest is the agile control of underactuatednon-holonomic systems, as the nature of these systems isthat certain degrees of freedom can only be controlled ina coupled manner (if at all). This makes it computationallyhard to determine simple and valid trajectories from scratch(Canny 1988; LaValle 2006), thus it is preferable to effi-ciently adapt a given trajectory in an elegant way withouthaving to start anew. This can be exploited for motionplanning by deriving new paths by refining previous ones,or within a feedback control loop to quickly compensatefor drift.

In cases where a suitable control strategy is known, pathcorrection is often performed by tracking in a pure pursuitmanner. However this can produce unwanted artefacts dueto its myopic nature (Kanayama et al. 1990; Ollero andHeredia 1995). Taking a larger horizon into account forregenerating a path increases computational complexity, butenables alterations to the path to be made in an elegant way.An example of such corrections is illustrated in Figure 1.This paper develops algorithms capable of quickly findingsuch path corrections without the need for explicitly givencontrol strategies such as cross-track steering methods.

Mechanical systems frequently exhibit symmetries thatcan be represented as groups of translation or rotation(Koon and Marsden 1995; Ostrowski 1999; Webster IIIet al. 2004) and allow for elegant trajectory correctionsin a computationally tractable way. This is valuable asthe degrees of freedom represented within these symmetrygroups are often those that are only indirectly modifiable,making the control problem more complex. For example,for most vehicles (be it submersible, ground or airborne)properties such as thrust, speed and turn rate can be influ-enced directly, whereas controlling position and heading isharder since they can only be influenced indirectly. As thelatter often exhibits aforementioned symmetries, efficientplanning algorithms for symmetry groups can be a valuabletool for dealing with problems that are difficult to handleotherwise.

1Australian Centre for Field Robotics, The University of Sydney, Sydney,Australia2The University of Queensland, Brisbane, Australia3National ICT Australia, Sydney, Australia

Corresponding author:Konstantin M. Seiler, The University of Sydney, The Rose Street BuildingJ04, The University of Sydney, NSW 2006, Australia.Email: [email protected]



152 The International Journal of Robotics Research 31(2)

Fig. 1. A car (small rectangle) is following a previously planned path (solid line) to the goal (dot), but got off-track due to disturbances.The left image shows a pure pursuit controller trying to get back on track as quickly as possible, resulting in unnecessary turns (dashedline). A more natural solution is shown in the second picture where the available space is used to elegantly correct the path during theupcoming turn.

The contribution of this paper is the introduction of theSymmetry Path Correction (SPC) algorithm that allows forplanning and control of systems that exhibit symmetriesand might be bound to non-holonomic constraints. It is arefinement of an idea presented by Cheng et al. (2008). Themethod works without prior knowledge of control strategiesspecific to the system at hand, by transforming an alreadyknown path such that it reaches a new goal. The method isdesigned such that it avoids excessive reintegration of theentire trajectory. In this paper, the discussion of Seiler et al.(2011) is extended in two ways: (1) for initial path plan-ning, the RRT Correct algorithm is developed as a novelvariation of the well known Rapidly Exploring RandomTree (RRT) algorithm that is able to incorporate SPC; (2)it is shown how SPC can be integrated as a controller intoa feedback control loop. Methods are developed indepen-dent of specific applications and demonstrated in simulationand experimentally using a ground vehicle and for surgicalneedle steering.

The remainder of this paper is structured as follows. Sec-tion 2 highlights previous work in the domain. Section 3develops the mathematical model and introduces the basicconcepts for trajectory alteration. Section 4 presents theSPC algorithm for situations without obstacles and showsresults of its application to (kinematic) car and flexibleneedle steering problems. This is extended to cases withobstacles in Section 5. Section 6 introduces the RRT Cor-rect framework to exploit path correction for motion plan-ning and its application for a car is demonstrated. In Sec-tion 7, SPC is integrated as feedback controller into thecontrol loop, an application which is demonstrated on aSegway RMP-based vehicle. Section 8 discusses imple-mentation issues resulting from discretization. We concludein Section 9. An introduction into Lie groups is given inAppendix A.

2. Previous work

LaValle and Kuffner (2001) introduced the Extend functioninto the RRT framework to deal with planning problemsunder non-holonomic constraints. This method is the basisof many state-of-the-art applications and variations. TheRRT Correct presented here extends this and allows one toutilize the power of a path correction algorithm by takinginto account the whole path from the tree’s root to a nodewhen extending it.

Lamiraux et al. (2004) proposed a path deformationmethod for obstacle avoidance based on potential fieldsin the space of paths that repels the path from obstacles.This approach works by linearly combining perturbations ofthe control input and simultaneously solving the boundaryvalue problem defined by the final state.

Cheng et al. (2008) used Lie group actions for gap reduc-tion as a post-processing step for RRT planning. Theyintroduced a method to insert coasting trajectories into anexisting trajectory in order to reduce gaps that arise dur-ing a planning phase. This approach is likely to performwell for twisted paths that feature segments going in anydirection, but for less-twisted paths there is no possibilityto shorten any part of an initial trajectory and recover fromovershooting. The SPC algorithm presented in this workovercomes this problem by actually altering existing seg-ments of the initial trajectory in a consistent manner. SPCalso introduces a new mechanism to select a valuable set oftransformations.

Pham (2011) used affine transformation for trajectorycorrection. While this approach has the ability to come upwith the transformed trajectory in one step, it does not takecare of generating the necessary controls in general. It relieson the ability to easily calculate valid controls for any paththat is twice differentiable.



Seiler et al. 153

3. Mathematical model

The following discussion is framed on the assumption thatan initial path has been obtained, but needs to be correctedtowards the desired goal. Such corrections might be nec-essary due to gaps arising from sampling-based planners,dynamic changes in the environment or due to disturbances.Furthermore, the algorithm is designed under the assump-tion that the initial path is up to some degree surroundedby free space, but may still contain narrow situations. Forease of presentation, this presentation at first concentrateson altering degrees of freedom represented by aforemen-tioned symmetry groups. It is understood that the remainingdegrees of freedom are dealt with via a classical planningor control methods. This is discussed in detail in Sections 6and 7.

It is assumed, that the reader is familiar with the notionof Lie groups. An introduction to the topic can be found inAppendix A; however, for more thorough treatment of thetopic, the reader is referred to the texts of Bloch (2003) andMurray et al. (1994) for example.

3.1. A system’s symmetries

When considering symmetry within this work, discretesymmetries such as the common mirror symmetry arenot considered. This work is targeted at symmetry that isformed by continuous symmetry groups. As an exampleconsider the system of a car on a plane as depicted in Fig-ure 2. Unless there are additional distinct features presentsuch as obstacles or boundaries, all possible positions ofthe car (x, y and heading) are equal. As a result, previouscalculations that are valid for one initial position can eas-ily be transformed to any other desired initial position. Thesystem’s behaviour relative to the initial position will beidentical. As a result, the car’s symmetry group is the spe-cial Euclidean group, SE( 2), consisting of all translationsand rotations on the plane. This kind of symmetry has beenused in the past, often implicitly without stating it directly.For example, implementations for planners working withdiscrete path sets often rely on this property when assum-ing that path sets are equally valid at every point (Chosetet al. 2005).

Symmetry formed by a Lie group can be observed notonly for car-like ground vehicles, but for a variety of dif-ferent systems. For example, steering a surgical bevel tipneedle through tissue (Webster III et al. 2004; Park et al.2010) is invariant under translations and rotations in R3

and thus the symmetry group equals SE( 3). In the case ofan aircraft in mid-air, the system is invariant under three-dimensional translations as well as yaw. Thus, the resultingsymmetry group equals R3 × SO( 2).

Similar symmetry phenomena arise for example whennot only taking into account static situations, but time andvelocities as well. For example, executing a plan at highervelocities results in either larger distances travelled or fin-ishing the route in less time. When describing this effect

in a space representing position, velocity and time, thearising transformations are known as Galilean transforma-tions. They form a Lie group known as the special Galileangroup, SGal( n) (Nadjafikhah and Forough 2007). Forexample, the planning problem of a robot having to inter-cept another object at a certain place and time could exhibita symmetry group based on SGal( n). While the applica-tions within this work only consider static symmetries basedon SE( n), the underlying concepts are derived and pre-sented in the context of Lie groups to allow a more generalemployment.

It is typical for systems with symmetries that the degreesof freedom formed by the symmetry group can only be con-trolled indirectly. That is due to the underactuated natureof the system and differential constraints, the dimensionsof the state space formed by the symmetry group can onlybe influenced via a set of differential equation by alteringthe state space’s remaining dimensions. For example, whendriving a car, speed and turn rate can be controlled directly,whereas position and heading have to be controlled indi-rectly by (often tedious) manoeuvring as can be seen atthe parallel parking problem for example. The same is truefor the other systems mentioned above. This justifies theneed to consider special planning and control strategies forhandling the symmetry group.

3.2. Basic definitions

This work assumes the notions and terminology associatedwith kinodynamic motion planning (Donald et al. 1993;LaValle and Kuffner 2001; LaValle 2006). The state spaceX is the space containing all possible states of the sys-tem. Each point in X corresponds to a set of values thatcompletely describe the relevant parameters of the system.Often this is a robot’s position in the environment, the posi-tions of the robot’s internal joints as well as derivativesthereof. While there are no requirements to the structure ofX per se, it will be assumed here that it can be represented asa differential manifold with a distance metric. This assump-tion is reasonable as it is true for a large class of motionplanning problems (LaValle 2006). In the case where Xcontains the robot’s position as well as the first derivativethereof, the velocity, X is also referred to as phase space(Donald et al. 1993).

The subset Xobs ⊆ X denotes the states that are notviable due to obstacle constraints, and its complementXfree := X \Xobs represents the viable free space. For clarityof presentation, at first an obstacle-free setting (Xobs = ∅)is assumed (cases with obstacles will be tackled from Sec-tion 5 onwards). The space U ⊆ Rn represents the system’sallowable control inputs. Thus, system progress is modelledvia a set of ordinary differential equations (ODEs)

x = F( x, u) (1)

for x ∈ X and u ∈ U (LaValle and Kuffner 2001).




Fig. 2. A car is sitting on a plane (left). The middle picture shows the car after it has moved. However, the resulting situation is stillequivalent to the first as can be seen easily by simply moving the camera (right). This is an easy example of the type of symmetryconsidered here.

The continuous symmetries as described informally inthe previous section are formalized by a Lie group G actingfreely1 from X from the left such that F( ·, u) are invariantvector fields under the action of G. That is, there exists amultiplication law for elements g ∈ G and x ∈ X , such thatgx ∈ X , and for every trajectory x( t) : I ⊆ R → X , andcontrol input u( t) : I → U fulfilling Equation (1), the prod-uct gx( t) also fulfils (1) for the same u( t). In addition, it isassumed that the metric on X is invariant under the actionof G. That is,

distance( x1, x2) = distance( gx1, gx2)

for all x1, x2 ∈ X and g ∈ G. Throughout this paper,symmetries that do not arise from the invariance of the dif-ferential equations as described here will not be considered.

This setting allows us to define the quotient space

Z = X�G

and thus a decomposition of the state space X in the form

X = Z × G (2)

where the manifold Z is the base space and the Lie groupG is denoted the fibre component. The projections fromX onto its components Z and G are denoted πZ and πG

respectively. Common examples of such invariantly actingLie groups arising from the system’s symmetry group, aretranslations (Rn), rotations (SO( 2), SO( 3)) or combina-tions thereof resulting in subgroups of SE( 3) (e.g. SE( 2),R3 × SO( 2), …). However, as seen in the previous section,other symmetry groups are possible, e.g. SGal( 3). Whilethe construction via a quotient space does not automaticallygive a reasonable presentation of Z in a constructive way,for conventional robots the decomposition of (2) tends tocome naturally when choosing a minimal parameterizationof the system.

Applying this framework to the example of a simple car(LaValle 2006) yields a state space X containing five dimen-sions, denoted speed v, turn rate ω, position x and y and

heading θ . The control inputs U contain two dimensions,acceleration a and change in turn rate ρ. The equations ofmotion are

v = a

ω = ρ

x = cos( θ ) v

y = sin( θ ) v

θ = ωv.

(3)

Limitations such as the maximum speed and turning ratecan be implemented as boundaries of X or obstacles,whereas a and ρ can be limited by the choice of U . Fur-ther constraints such as those on a and ρ as well as slip areneglected. As discussed above, the car’s behaviour is inde-pendent of position and heading and the system’s symmetrygroup G equals the special Euclidean group, SE( 2), repre-senting x, y and θ . The remaining base space Z is spannedby v and ω. Thus

X = R2 × SE( 2) .

Introducing some notation simplifies matters. Let I ⊆ R

be a closed finite interval. Then I− and I+ denote the lowerand upper boundary values, respectively, such that

I = [I−, I+].

A time-dependent control input is considered to be afunction u : Iu → U that maps from a closed finite intervalIu ⊆ R into the control space U . Integrating such a con-trol input over time via the ODE (1) gives rise to a path instate space X that is dependent on an initial state x0 andtime t. Such integrated paths can be written as functions�u( x0, t) : X × Iu → X with the properties

�u( x0, t)= F( �u( x0, t) , u( t) )

and

�( x0, I−u )= x0 .



Seiler et al. 155

Given two time-dependent control inputs u : Iu → U andv : Iv → U with I+u = I−v , u ∗ v : [I−u , I+v ]→ X is definedas the concatenation of the two functions u and v such that

( u ∗ v) ( t)={

u( t) , if t ∈ [I−u , I+u )v( t) , otherwise.

This notation may also be used in cases where I+u �= I−v . Inthese cases a suitable re-parameterization of Iv is performedimplicitly. Note that when using this notation for two inte-grated paths in state space X , the concatenation results ina single continuous path if and only if the final state of thefirst path coincides with the initial state of the second path.When this is the case, the resulting path is equivalent tointegrating the concatenated control inputs directly, thus

�u∗v( x0, t)= �u( x0, t) ∗ �v( �u( x0, I+u ) , t) .

3.3. Trajectory transformations

In general, finding a solution for the planning problem ofconnecting two predefined points xstart and xgoal in X leadsto running a search over all time-varying control inputs(Canny 1988; Latombe 1991). As the space of all possiblecontrol inputs can be too big to search exhaustively, manyalgorithms focus on relatively small subsets and either run asearch over a discrete set of paths (Choset et al. 2005) or runa non-linear optimization algorithm or search over a contin-uous path set (Lamiraux et al. 2004; Kobilarov et al. 2007).The former, by its very nature, can only reach a discretesubset of X , where as the latter typically involves reinte-grating the whole trajectory �u( xstart, t) in each step of theoptimization process.

Using operations given by a Lie group to transform avalid trajectory allows for the reuse of large parts of a pre-viously calculated �u( xstart, t) as long as changes to thetrajectory happen in a compatible way. Thus, searching acontinuum can be done without complete reintegration.

Let u and v be two time-dependent control inputs thatdiffer in some region, but coincide otherwise. They can besplit up as

u = u1 ∗ u2 ∗ u3

andv = u1 ∗ v2 ∗ u3

where u1 and u3 represent the parts that are common to both.Note that the lengths of the middle segments Iu2 and Iv2 donot necessarily have to be equal. Starting both trajectoriesat a common initial state x0 ∈ X yields

�u( x0, t)= �v( x0, t) for t ∈ Iu1 .

In general, equality of the third part of the control inputs, u3,cannot be used, as the final states of the middle segments,�u( x0, I+u2

) and �u( x0, I+v2), need not coincide. Using differ-

ent states as initial states for the third part of the path canresult in a variety of different trajectories as illustrated inFigure 3.

Fig. 3. Three trajectories for a car (rectangle), all resulting fromthe same control input. The behaviour is sensitive to initialconditions (speed and turn rate), causing different trajectories.

Fig. 4. A car (rectangle) follows two different paths resultingfrom control inputs that only differ on a region in the middle butare identical otherwise. The paths coincide up to the first marker(dot). After that, the paths differ. However at the respective secondmarkers, speed and turn rate are identical for both paths and thusthe remaining parts of the paths are the same, just transformed.

If, however, it is assumed that the final states of the mid-dle segments u2 and v2 only differ on the fibre component Gbut coincide on the base space Z, the similarity of the thirdparts of the trajectory can be exploited. Having equality onthe base space as in

πZ( �u( x0, I+u2))= πZ( �v( x0, I+v2

)) (4)

implies there exists a transformation g ∈ G such that

�v( x0, I+v2)= g�u( x0, I+u2

) . (5)

In the case of the simple car, Equation (4) can be inter-preted as having identical speed v and turn rate ω. ThenEquation (5) yields the translation and rotation necessaryto transform one state into the other. Because the equa-tions of motion are invariant under translation and rotation,the resulting third parts of the paths will be translated androtated versions of each other as illustrated in Figure 4.

In the general case, the same line of reasoning on invari-ance yields

�u3 ( �v( x0, I+v2) , t) = �u3 ( g�u( x0, I+u2

) , t)

= g�u3 ( �u( x0, I+u2) , t) . (6)

Looking at this result from a viewpoint of computationalcomplexity, Equation (6) saves calculation time. Given �u,the computational cost of �v is mainly the cost of inte-grating the second segment given by v2. The third segment




defined by u3 can be calculated directly by the use of groupoperations. In particular, during non-linear optimization,the final state �v( x0, I+v ) is typically the only one of interest.Thus, there is no need to actually transform the whole thirdsegment of the path. Instead one can determine the trajec-tory’s final state directly. As a result, the cost for �v( x0, I+v )is linear in the size of Iv2 .

3.4. Optimizing a trajectory

While the overall framework is agnostic to the specificnature of a trajectory transformation (as long as it canparameterized in a sufficiently smooth way and Equation(4) is met), this work focuses on operations that can bestbe described as stretching. Given a time-dependent controlinput u and a corresponding trajectory �u( x0, t), one mightbe able to find an alteration uc that stretches (or compresses)the trajectory’s behaviour on the base space Z over time.That is

πZ( �uc ( x0, t) )= πZ( �u( x0, ct) ) (7)

for a stretch factor 0 < c ∈ R. In particular, this yieldsidentical final states on Z,

πZ( �uc ( x0, I+uc ) )= πZ( �u( x0, I+u ) ) .

In the case of the car, for instance, this could map toreduced accelerator commands resulting in a longer dis-tance travelled by the time the target speed is reached.While the stretching operation does not change the endresult on the base space Z, it does alter the position onthe fibre G, thus emphasizing or weakening features of thetrajectory. For the car, the stretching operation can be cal-culated by dividing the control inputs a and ρ by c whilemultiplying the time they are applied by c.

This kind of stretching operation is always possible forsystems where the base space Z is fully actuated. In case ofan underactuated base space however, it might not be pos-sible to generate the necessary controls. While these casesmight still be feasible by introducing other kinds of trans-formations that fulfil Equation (4), such cases are beyondthe scope of this work.

Combining the results obtained so far, an efficient tool foraltering a trajectory during a non-linear optimization pro-cess can be built. Let �u( x0, t) be a trajectory given by asplit control input

u = u1 ∗ . . . ∗ un

and starting point x0 ∈ X . Changing a single ui to ucii =:

vi results in a Lie group operation gi ∈ G as of Equation(5). Repeating this, one is able to alter several or even allsegments of the path at once in order to get a new controlinput

v := v1 ∗ . . . ∗ vn

where all vi result from some ucii . In cases where ci = 1,

and thus the segment is unaltered, the corresponding gi is

Fig. 5. A scaling operation has been applied to the bold seg-ments of the top path to derive the bottom trajectory. Only the boldsegments had to be reintegrated, the remainder is identical.

set to the identity element 1 ∈ G without further calcula-tion. Assuming �u( x0, t) is given and the changed segments�vi( �u( x0, I+ui−1

) , t) and transformations gi have been cal-culated, the new trajectory �v( x0, t) is computed effi-ciently using group operations only. Iteratively applying (6)yields

�vi( �v( x0, I+vi−1) , t)= gi−1 . . . g1�vi( �u( x0, I+ui−1

) , t)

and, thus,

�v( x0, t)= �v1 ( x0, t) ∗g1�v2 ( �u1 ( x0, I+u1) , t)

∗ . . . ∗ gn−1 . . . g1�vn ( �un−1 ( x0, I+un−1) , t) .

In particular, one can write the final state of the trajectoryas

�v( x0, I+v )= gn . . . g1�u( x0, I+u ) . (8)

Clearly, not much is saved in cases where all segments ofthe trajectory have been changed (i.e. all ci �= 1). However,if only a small fraction of the control input has been altered,then it is only necessary to reintegrate the fibre componentof those altered segments. Thus, the computational cost forcalculating the new trajectory, or directly its end point, islinear in the length of the changed segments plus the cost ofa few additional group operations.

Note that it is possible to perform the calculations of gi

and �vi ( �u( x0, I+ui−1) , t) separately for each segment, inde-

pendent of what is done to other segments. Thus, for anothertransformation using some c′i, all results where c′i = ci canbe reused. This speeds up things significantly for gradientcalculations as will be detailed later and also allows forparallel computation.

4. Symmetry Path Correction algorithm with-out obstacles

For path correction, it will be assumed that an initial path�u( x0, t) as well as its control input u and initial statex0 have been given. Furthermore, the path’s final state



Seiler et al. 157

Fig. 6. The segments (bold) chosen for the trajectory on the left are unable to span the space well as they enable moving the final statehorizontally and vertically, but prohibit alteration to the car’s heading. The selection shown on the right is superior because changes inall directions including heading are possible.

�u( x0, I+u ) does not coincide with the goal xgoal, but is inthe vicinity of it. The objective is to alter the trajectory �u

in such a way that its final state matches xgoal. It will beassumed that the correction needs to be done in the fibrecomponent only and that there are no obstacles present.This will be achieved in two steps: first, a small and suitableset of path segments will be selected for stretching oper-ations; and, second, matching stretching factors ci will bedetermined for said segments.

When selecting path segments, it is advantageous toselect exactly as many segments as there are dimensionsin the Lie group G. Using less segments results in too fewdegrees of freedom when altering the trajectory and thusfailure to span a whole neighbourhood of the final state�u( x0, I+). Using more segments than dim G leads to unde-sired behaviour as the solution is no longer unique. Further-more, segments are chosen in such a way that the directionsthey move the trajectory’s final state into have the potentialto span the space well as illustrated in Figure 6. This can beformalized by considering the derivatives

∂�v( x0, I+v )

∂ci= ∂gi�u( x0, I+u )

∂ci

evaluated at ci = 1. As above, v represents the controlinput u with some segments ui replaced by their scaled ver-sions uci

i and, again, gi ∈ G denotes the resulting Lie grouptransformation.

The quality of a selection of dim G segments can thenbe measured by analysing the condition number of theresulting Jacobian

J = ∂�v( x0, I+v )

∂( c1, . . . , cdim G)

=(

∂g1�u( x0, I+u )

∂c1, . . . ,

∂gdim G�u( x0, I+u )

∂cdim G

)(9)

evaluated at ci = 1 for all i. The derivatives in the matrixon the right-hand side are written as column vectors. If thematrix’s condition number is small, it has the potential tospan the space well.

Since each column of J in Equation (9) is independent ofthe remaining segments, the derivative has to be calculatedonly once. Thus, in practice, a method is to select a largerset of non-overlapping segments and out of that randomlydraw selections of dim G elements for further testing. Theselection with the smallest condition number of the result-ing Jacobian is then chosen. An exhaustive search for theoptimal selection is not necessary since it is sufficient toremove poor candidates. Taking a few random samples isoften enough.

As optimization algorithms typically work by minimiz-ing a target function (Byrd et al. 2006), here the distance ofthe path’s final state to the goal, it might seem tempting toestimate the convergence rate of that target function directlyvia its second-order approximation and the eigenvalues ofthe Hessian (Shewchuk 1994; Cheng et al. 2008) insteadof using the condition number of the final state’s Jacobianas presented here. In tests, however, this proved to performpoorly.

Once a set of segments is chosen, the values for theci need to be determined in order to actually improvethe trajectory. Therefore, a target function f ( c1, . . . , cdim G)is defined as the distance between �v( x0, I+v ) and xgoal.It is then minimized using a conjugate gradient method(Shewchuk 1994; Byrd et al. 2006). Estimating the gra-dient of f at ( c1, . . . , cdim G) is done by taking intoaccount the function value f ( c1, . . . , cdim G), as well asthose resulting from going a small step into each direction,f ( c1, . . . , ci + ε, . . . , cdim G), naively resulting in dim G+ 1integrations for each segment. However, since only two dis-tinct values, ci and ci+ε, are used for each dimension of G,the calculated gi can be recombined to obtain all functionevaluations necessary. Thus, the cost to estimate a gradientis two integrations per segment plus some group operations.Pseudocode for the resulting Symmetry Path CorrectionAlgorithm (SPC) is presented in Algorithm 1. It was usedfor the path depicted on the right-hand side of Figure 1 aswell as the example presented in Figure 7.

Implementing this algorithm for more complex three-dimensional cases, such as bevel tip needle steering, the




Fig. 7. A car (rectangle) is trying to reach the goal (dot). Thedashed line shows the initial path that fails to reach the goal. Byaltering the segments depicted in bold, the solid path is created.

Algorithm 1 Symmetry Path Correction Algorithm (SPC)without obstacles

u← current planM ← set of at least dim G disjoint segments of Iu

Cmin ←∞for i = 1 to maxSelections do

S← draw selection of dim G random elements of M

J ←(

∂g1�u(x0,I+u )∂c1

, . . . , ∂gdim G�u(x0,I+u )∂cdim G

){calculated for the segments stored in S}

if cond( J ) < Cmin thenCmin ← cond( J )Smin ← S

end ifend foroptimize c1, . . . , cdim G

v← scale the segments of u stored in Smin

according to values of c1, . . . , cdim G

until dist(xgoal, �v( x0, I+v )

)minimal

return v

state space X consists of eight dimensions: insertion speedv, turn rate ω as well as six degrees of freedom representingposition and orientation in three space. Thus, the basespace Z represents v and ω whereas G equals the specialEuclidean group SE( 3). Following previous notation in thisdomain (Webster III et al. 2004; Park et al. 2010), SE( 3) isrepresented using homogeneous 4 × 4 matrices g = (

R t0 1

)where R ∈ SO( 3) is a rotation matrix and t ∈ R3 representstranslation. The control space U has two dimensions,acceleration a = v and change in turn rate ρ = ω. Theremaining equations of motion are given by

g−1g =

⎛⎜⎜⎝

0 −ω 0 0ω 0 −κv 00 κv 0 v0 0 0 0

⎞⎟⎟⎠ ∈ se( 3)

where the constant κ is the curvature of the needle’s trajec-tory and se( 3) is the Lie algebra of SE( 3). An example ofSPC operating on this system is presented in Figure 8.

Since the algorithm works by enlarging or shrinking cer-tain sections of the trajectory, it cannot perform well in

Fig. 8. Path correction for a needle steering case. The needleneeds to reach the goal (dot), but the initial plan, depicted by thedashed line, misses it. The solid line is the correction made by thepath correction algorithm.

cases where the trajectory has too few features. In partic-ular, in cases where the path consists only of a straight lineor a section of a circle, it is impossible to find segments thatspan the space well in a way discussed previously.

5. Symmetry Path Correction algorithm withobstacles

When dealing with obstacles, an inversion of perspectiveis helpful. Up to now the initial path was considered tostart at the robot’s current state and the final state was thenoptimized. However, it is equally valid to anchor the initialpath at the goal state xgoal and consider the robot to be at astate xcurr that does not coincide with the path’s initial statexstart = �u( xstart, I−u ).

It will be assumed that the initial path is a collision-freetrajectory �u( xstart, t) with �u( xstart, I+u )= xgoal. Here Xobs

does not have to be empty, but it is assumed that there isa certain amount of free space surrounding the trajectorymost of the time that can be used for corrective actions. Therobot is currently at xcurr and is following a path defined byv that is derived from, but that might not coincide with u. So�v( xcurr, t) does not necessarily reach the goal xgoal. Allow-ing a discrepancy between u and v is advantageous whenmaking multiple corrections; for example, when repeatedonline calculations are performed while executing a pathunder disturbances. In this setting, the initial path u will bekept constant during the whole process, while alterationsare made to v only.



Seiler et al. 159

If no collisions occur in �v( xcurr, t), alterations to vcan be made directly using SPC without obstacles (Algo-rithm 1). Otherwise, in case of collisions, the first point ofcollision is found as

tcol := min{t ∈ Iv | �v( xcurr, t)∈ Xobs}and the path can be split such that v = v1 ∗ v2 and tcol = I+v1

.To get around the obstacle, it is necessary to correct the pathin two steps, first �v1 ( xcurr, t) using a new intermediate goalxnew ∈ Xfree and then v2.

To define xnew, the colliding point �v1 ( xcurr, I+v1) is pulled

towards the corresponding point xorig of �u( xstart, t) where,due to scaling operations performed on v, the time t isnot necessarily identical for u and v any more. This isdone by parameterizing a straight line connecting xorig with�v1 ( xcurr, I+v1

) by s : [0, 1]→ X such that s( 0)= xorig ands( 1)= �v1 ( xcurr, I+v1

).2 Using s, the intermediate goal isdefined as

xnew := s( αn) ,

where n is the smallest n ∈ N such that s( αn)∈ Xfree

and α ∈ [0, 1). The convergence rate α determines howfast the trajectory should be pulled back towards the origi-nal path and away from the obstacle. A large α results instaying closer to the obstacle and thus in a higher prob-ability that further corrections are necessary; a smaller α

on the other hand pulls the trajectory more aggressivelytowards the original trajectory, preventing effective use ofthe available free space. The presented implementation usesα = 1/2.

Using xnew, SPC without obstacles is run over v1 to get anew plan w1 and thus w = w1 ∗ v2. Note that it is acceptableif the new trajectory does not actually reach xnew itself asthe main purpose of the operation is to pull the path awayfrom the obstacle. What has to be considered though arecollisions of �w1 ( xcurr, t). If �w1 ( xcurr, t) is collision free,the process continues by recursively applying SPC withobstacles on v2. If �w1 ( xcurr, t) is in collision, two caseshave to be considered. If the final state �w1 ( xcurr, I+w1

) isin collision, the optimization run did not get close enoughto xnew because the path given by v1 was too short or hadtoo few features for the algorithm to perform well. Incases where v1 is not the first part of the path due to arecursive call, w (and thus v1) can be extended and a newattempt can be made. Otherwise the system is too closeto an obstacle for suitable correction, and the algorithmis considered failed. If the final state �w1 ( xcurr, I+w1

) is inXfree, the optimization run was successful but the alterationintroduced a new collision. Then a recursive call on thealtered plan w and the starting point xcurr is necessary toget rid of the newly introduced collision. Pseudocode forthe complete SPC with obstacles is given in Algorithm 2.An example of the algorithm’s performance for a needlesteering problem is shown in Figure 9.

Note that the sole purpose of the original path u is toprovide a reference path when selecting intermediate waypoints xnew. Keeping an initial path anchored at xgoal for thispurpose has shown to provide good results, but in cases

Algorithm 2 Symmetry Path Correction Algorithm (SPC)with obstacles

u← original planv← current planxcurr ← system’s current stateS← empty stackrepeat

if �v( xcurr, t) in collision thenv1, v2 ← v split at point of first collision

elsev1, v2 ← v,∅

end ifxnew ← find intermediate goal using uv1 ← run SPC without obstacles (Alg. 1)

for �v1 ( xcurr, t) in order to reach xnew

if �v1 ( xcurr, t) collision free thenS. push← v1, xcurr

v, xcurr ← v2, �v1 ( xcurr, I+v1)

else if �v1 ( xcurr, I+v1) collision free then

v← v1 ∗ v2

else if S not empty thenv0, xcurr ← S. popv← v0 ∗ v1 ∗ v2

elsereturn FAIL

end ifuntil v = ∅while S not empty do

v0, xcurr ← S. popv← v0 ∗ v

end whilereturn v

where this approach is not available, other choices for ucan be made. Such a case will arise in the context of thefollowing section.

6. Integrating path correction into motionplanning

There are multiple ways by which an off-line planner canbenefit from a path correction. For example, it can be usedas a post-processing step for gap reduction to deal withdiscontinuities after the planner has completed. Sampling-based planners such as RRT, or those based on discrete pathprimitives such as Grid-Based Search (Choset et al. 2005),tend to leave small discontinuities in a path due to their dis-crete sampling nature. A path correction algorithm can beused to reduce these gaps resulting in a higher quality pathand in faster planning due to the possibility of allowing big-ger gaps during the initial planning phase. This has beenconsidered by Cheng et al. (2008).

In this work however another approach is taken to inte-grate the path correction algorithm directly into an RRTstyle planner. To do so, a variation of the RRT algorithm




Fig. 9. A needle has to reach a goal (dot) and has an initial pathdepicted by the dashed line. Owing to an offset in the initial posi-tion, the planned trajectory would result in the dotted line, collid-ing with an obstacle and missing the goal. The path is correctedand a valid path depicted by the solid line is created.

was derived. The most basic form of an RRT planner asdescribed in literature, e.g. in LaValle (2006), works as fol-lows: in each iteration, at first, a new point of the state space,xsample ∈ X , is sampled and the node closest to it in thesearch tree, xclosest ∈ T , is determined. Then the Connectfunction, a local planner, is employed to create a path vfrom xclosest to xsample. Finally, the result of the local plan-ner is added to the tree, extending T at xclosest by an edgethat ends at xsample. Typically, this is relaxed slightly by notrequiring the new edge to actually reach xsample, but someother point xnew instead, making only some progress towardsxsample. Throughout this process, the local planner is con-sidered as a black box that takes two points as an input andcreates an edge leading from the first point towards the sec-ond. This is repeated until a point is added to the graph thathas a distance to the goal that is smaller than some goalacceptance threshold ε. Pseudocode for this basic RRT isgiven in Algorithm 3.

While for the holonomic case, the local planner is fairlyeasy to implement, finding a suitable local planner fora non-holonomic system with differential constraints is a

Algorithm 3 A basic RRT algorithm.xstart ← the start positionxgoal ← the desired end positionT ← tree initialized with node xstart

repeatxsample ← sample new point in Xxclosest ← arg minx∈T distance( x, xsample)xnew, v← Connect( xclosest, xsample)add xnew as new node to Tadd edge v from xclosest to xnew to T

until distance( xgoal, xnew)≤ ε

return path from xstart to xnew

Algorithm 4 The RRT Correct algorithm.xstart ← the start positionxgoal ← the desired end positionT ← tree initialized with node xstart

repeatxsample ← sample new point in Xxclosest ← arg minx∈T distance( x, xsample)vclosest ← path from xstart to xclosest

xnew, v← ExtendCorrect( xclosest, vclosest, xsample)add xnew as new node to Tadd edge v from xstart to xnew to T

until distance( xgoal, xnew)≤ ε

return v

hard problem which lead to the development of the RRTExtend algorithm (LaValle and Kuffner 2001). In RRTExtend, the Connect function is replaced by an Extendfunction that only attempts to do some progress towardsxsample, instead of an actual connection. As a result it is bydesign very unlikely to exactly reach the given point xsample,and almost certainly settles with incremental progresstowards it.

When incorporating path correction into this process,it is advantageous to slightly change the role of the localplanner. To do so, the Connect function is replaced by theExtendCorrect function which takes as input not only theclosest point xclosest, but also the whole path vclosest thatconnects xstart to xclosest. The local planner will then extendand/or otherwise alter vclosest such that it reaches xsample orat least does some progress towards it. The result is a path vstarting at xstart leading towards xsample that does not neces-sarily pass through xclosest any more. Extending the tree byv, results in an edge starting directly at xstart. Note that thisbehaviour of always extending the tree at its root effectivelydegenerates the tree structure resulting in something look-ing more like a bouquet of flowers than an actual tree. It canbe conveniently stored in a list or array, saving the complex-ity of dealing with a tree structure. However, this comes atthe additional cost of having to store the whole path withevery node. Pseudocode for this RRT Correct algorithm isgiven in Algorithm 4.



Seiler et al. 161

Algorithm 5 The ExtendCorrect functionxclosest ← The given point the extension should start atvclosest ← The path from xstart to xclosest

xsample ← The sampled point that should be reachedx, v← Extend( xclosest, xsample)v← vclosest ∗ vu← vrepeat

v←run SPC with obstacles using v as current plan,xsample as goal, and u as original plan

until no sufficient progress was madexnew ← end point of vreturn xnew, v

Note that the Correcting RRT algorithm is a generaliza-tion of usual RRT, as the latter is equivalent to the casewhere the ExtendCorrect function only extends the previ-ous path vclosest without altering it. Also note that while thiswork describes a basic single RRT implementation, com-mon extensions such as growing multiple trees or advancedsampling strategies can be applied to the Correcting RRTframework in the usual fashion.

The idea behind the ExtendCorrect function is to havethe local planner perform two steps: first, it extends thegiven path vclosest to a point that is close to xsample; second,it applies the path correction algorithm to the extended pathto alter it in such a way that it actually reaches xsample.

The first step, the extension stage, can be performed inthe same fashion as for a normal RRT Extend implementa-tion. The implementation presented here uses a finite libraryof precomputed paths to select from. The second step, thecorrection stage, consists of a (possibly repeated) applica-tion of SPC with obstacles. At first, the original plan u ofAlgorithm 2 is initialized with the whole, extended, path asgenerated in the extension stage. This will stay unchangedeven if the correction algorithm is run several times in arow. The current path is used as original plan u, becauseother choices anchored in xnew are likely to cause colli-sions. It has proven beneficial to perform several iterationsof the SPC as long as significant progress towards xnew ismade. The implementation presented here uses the sim-ple heuristic that another run is employed if the distanceto xnew is still greater than the goal acceptance toleranceε and the previous run was able to reduce the distance toxnew by at least 30%. Pseudocode illustrating the wholeExtendCorrect algorithm is given in Algorithm 5.

The performance and completeness of the CorrectingRRT algorithm is no worse than the underlying RRT. With-out the path correction stage, the algorithm is equivalentto the RRT Extend algorithm, which is probabilisticallycomplete when some assumptions are made (LaValle andKuffner 2001). Also in cases of very cluttered environmentswithout room for path corrections, the correction phase onlyadds overhead to an RRT Extend by adding futile optimiza-tion attempts. However, in environments containing open

spaces, the power of the path correction algorithm allowsfor a much more efficient and precise connection showing abehaviour often similar to RRT in the holonomic case. As aresult, the Correcting RRT performs like an ordinary RRTExtend in the worst case but has the ability to do signifi-cantly better in good-natured environments with sufficientroom for path correction.

The Correcting RRT algorithm has been implemented forpath planning for a car on a plane with obstacles. It wasimplemented such that it does not use any explicit trajectorysolving algorithms that are known for cars, such as ReedShepp curves. For planning on the fibre, only the differentialequations given in (3) are used, for planning on the basespace, a pure pursuit planner/controller is utilized to assistthe planning process.

The Extend function is realized using a discrete libraryof pre-computed path samples. The paths in the library allstart with the same fibre component g0 ∈ G, but use varyinginitial values on the base space. Due to the symmetry, eachpath can be easily transformed to start at any other pointg ∈ G. In order to connect xclosest to xsample, a path v withinitial state xv and final state �v( xv, I+v ) is chosen from thelibrary, such that its initial state closely matches the basespace of xclosest and its transformed final state is close toxsample. That is,

distance(πZ( xclosest) , πZ( xv)

)

and

distance(g0πG( xclosest)

−1 πG( xsample) , πG( �v( xv, I+v ) ))

are both simultaneously small.Once the path is selected, the gap in the base space has to

be closed. This is done by simulating a pure pursuit con-troller tracking the base space and integrating the resulton the fibre component. While this, in theory, has unpre-dictable results on the final state of the path, �v( xv, I+v ), inpractice if the gap was small enough, the changes to thefinal state are rather small too. While this choice of Extendfunction seemed reasonable for this implementation, thealgorithm does not depend on any specific properties ofthe Extend function presented here and as such it can bereplaced by anything that seems to fit the problem at hand.

For experiments, the goal acceptance tolerance ε wasset to 0.01 (distances are measured in meters, angles inradians). It is observed, that once the search reaches thevicinity of xgoal, the SPC algorithm is often able to per-form the connection to xgoal directly. Further, the finallyaccepted state typically is by several orders of magnitudecloser to xgoal than ε due to the ability of the Extend-Correct algorithm to analytically reach a given point.Examples of the algorithm’s performance are given inFigure 10.




Fig. 10. A run of the Correcting RRT algorithm for a car navigating on a plane with obstacles. The marked positions are the initialstate in the center as well as the goal state in the bottom left corner. The accepted path is shown in both pictures. The left-hand picturedepicts the bouquet of all paths that were generated during the process. The right-hand picture illustrates the underlying tree structureby connecting each node with the one it was derived from. Note that the accepted path no longer passes through its parent nodes.

7. Using path correction for tracking

As discussed earlier, it is typically harder to control thefibre component G than the base space Z. Controllers forZ are often readily available or easy to implement and inmany cases, a standard PID controller is sufficient. In con-trast, controlling G often requires nuanced and tuned con-trol strategies or, in their absence, a complete replanningstep which can be resource intensive (Kelly and Nagy 2003;Wong 2008). To avoid replanning from scratch, path correc-tion can be used to account for deviations made while fol-lowing the fibre component of a path in an open-loop fash-ion. This closes the loop and effectively creates a feedbackcontroller for G.

Initially, an off-line planner derives a valid path �v

from the robot’s current position xcurr to the goal xgoal. Itis assumed that there is no necessity to actually follow�v closely as long as the system stays clear of obstacles.Throughout the whole process, the initial path is kept as areference path denoted �u. Thus, at the beginning �u =�v, but �v shall change later on.

Here �v is the loaded into the base space controller thatstarts tracking the base space component of �v in a closed-loop fashion, resulting in purely open-loop tracking of thefibre component. As soon as deviations in the fibre compo-nent are detected, a correction cycle is triggered using SPCwith obstacles (Algorithm 2). The algorithm is employedusing the inputs �v as current plan and �u as original plan.After the path correction succeeds, �v is updated with theresult and the base space controller continues tracking thenew �v until the cycle repeats. Schematics for a systemusing this setup are shown in Figure 11.

Keeping the initial path �u during the whole processgreatly benefits the algorithm’s ability to avoid obstacles.Updating it by �v in each cycle and thus using the currentpath as reference path for the next correction step is likelyto result in failure due to the fact that �v often gets veryclose to obstacles and thus all intermediate way points willbe chosen close to obstacles too. Also note, that the algo-rithm has no robustness guarantees. So the system has to beprepared to stop and replan from scratch in cases where thepath correction algorithm fails.

An experimental system using this setup for path track-ing has been implemented using the Segway RMP-basedground vehicle shown in Figure 12. A laser range finder wasused for self-localization and all calculations where per-formed using an on-board personal computer. The systemoperated in a flat environment containing various obsta-cles. The programming used the equations of motion only:no use was made of known explicit control strategies forground vehicles such as pure pursuit or explicit trajectoryschemes such as Reed Shepp. In addition, with the excep-tion of the very start of a trajectory, use of the Segway’s spotturn ability was prevented.

The equations of motion for this system differ slightlyfrom those of a car and are as follows. Again a and ρ arethe control inputs.

v = a

ω = ρ

x = cos( θ ) v

y = sin( θ ) v

θ = ω



Seiler et al. 163

Offlineplanner

Base spacecontroller Actuation

SPC Localization

upda

tedplan

fibre component

base space

original

plan

Fig. 11. Schematics for a system using Symmetric Path Correction as feedback controller.

In order to reach a given goal, the system first employeda grid-based search (Choset et al. 2005) to create an initialvalid path. While tracking the path, the base space vari-ables, speed and turn rate, are tracked using the Segway’sbuilt in controller whereas the fibre component is treatedin an open-loop fashion at first. After a short interval offollowing an open loop (here 0.5 seconds), SPC with obsta-cles is executed in order to correct the plan taking intoaccount the vehicle’s current position as determined by thelaser range finder. After completion, the path is updatedand tracking continues. Only in case of failure, that is anunresolvable collision, the system is stopped and a newinitial plan is derived from scratch. Examples of the sys-tem’s performance are shown in Figure 13. Video footageshowing the example of Figure 13 in full length is providedin Extension 1. The vehicle is tracking the dark red path.The fibre component is tracked in an open-loop fashion,whereas the base space is tracked closed loop. The pro-jected path, depicted in green, indicates the tracking errormade so far by illustrating what would happen if (idealizedand error free) tracking of the dark red path continued. Itcan be seen how the SPC algorithm repeatedly alters the tra-jectory in order to compensate for tracking errors and thuscloses the loop for the fibre component. Furthermore, it canbe seen how the algorithm makes use of free space whereavailable to perform the necessary corrections. This canresult in large deviations from the original trajectory. Thiseffect could be limited, e.g. by introducing artificial obsta-cles, however this approach was not implemented here. Theoriginal path, depicted in blue, stays unaltered throughoutthe whole process and is used as reference path to selectintermediate way points.

8. Discretization issues

Most of the above has been formulated using continuousfunctions without taking into account discretization issues.This section aims to address problems that might arise foractual numeric implementations.

Fig. 12. A Segway RMP-based autonomous ground vehicle usedfor experiments.

Typical implementations for integrating a trajectory oper-ate using fixed time steps. As such, the control inputs arediscretized using intervals of fixed duration and each inte-gration step integrates over one such interval. This approachhowever does not meld with the scaling operations central tothe methods of this work. This is because after scaling, thepoints in time where the control inputs need to change donot coincide with the interval boundaries any more and thelength of a trajectory segment might not even be a multipleof the interval size.

As a result, the integration routine needs to be imple-mented using variable step sizes. This allows the scalingoperation to be implemented in such a way that it directlyaffects the step size. As a result, control inputs that belongedto one interval before scaling, again form an interval after




Fig. 13. The robot (red dot, right) is tracking a path towards agoal (red dot, left). The original initial path as planned by the off-line planner is depicted in blue and kept constant throughout theprocess. Owing to open-loop tracking errors, the robot gets offtrack. The resulting projected path that does not reach the goal isshown in green. After application of the path correction algorithm,the green path was transformed into the dark red path that reachesthe goal and was used to proceed further. The process is repeatedcontinuously. Video footage showing the experiment in full lengthis provided in Extension 1.

the alteration. Also the duration of the segments is not lim-ited to multiples of the interval size. Doing so of courserequires a more careful design of the integration routine.In particular, when the algorithm is applied several timesto one trajectory, sampling intervals that were reasonablesmall originally can grow quite large over time. This needsto be accounted for within the integration routine, forexample by computing more sub-steps internally. However,herein lies another danger.

In order for the non-linear optimization to work well,the objective function needs to be smooth. While today’ssolvers can cope with small discontinuities, they can stillimpact the performance significantly, especially when theyare close to the optimum. Thus, one should design the inte-gration routine in such a way that uc as a function dependingon c is smooth. Discontinuities can, for example, be intro-duced by the integration routine changing the number ofinternal sub-steps during the process. As a compromise

between the desire to change the number of internal stepsin order to keep the results accurate and the desire to pre-vent discontinuities near the optimum, the implementationpresented here uses hysteresis to prevent frequent switchingnear the optimum. This has in fact proven to affect run-time significantly in some cases. Values around factor twohave been observed, but are obviously very dependent onthe specific implementation and problem.

9. Conclusion

The SPC algorithm is presented and allows for elegant andfast path correction while preserving the character of theinitial trajectory, thus eliminating the need for expensivereplanning from scratch. SPC has a wide range of possibleapplications in motion planning and control, two of whichwere presented here. The Correcting RRT algorithm wasdeveloped to utilize SPC for initial motion planning. It wasdemonstrated by growing a single forward tree to explorethe space. It was seen that the Correcting RRT has theability to exactly reach a sampled point, a property that isusually not present for an RRT in a non-holonomic setting.Future applications will incorporate common extensionssuch as growing multiple trees (e.g. Bi-RRT) and advancedsampling strategies.

SPC was integrated into a control loop allowing it toact as a tracker. The method has been implemented insimulation for a simple car and surgical needle steering,as well as on a physical ground vehicle. The method wasable to correct paths without explicit knowledge of controlstrategies suitable for the fibre component.

The results show that integrating symmetry-based pathcorrection into a planning algorithm allows us to reducecomputational complexity and thus is a promising approachfor problems that otherwise cannot be solved due toresource limitations. Future work will include implement-ing these methods for a wider range of platforms. In partic-ular, this will involve considering symmetries that are notbased upon SE( n) as well as incorporating a wider rangeof transformations to tackle systems with an underactuatedbase space.

Notes

1. The action of G on X is free if and only if for all x ∈ X andg, h ∈ G it is true that gx = hx implies g = h. Althoughcounter-examples exist, if G is a symmetry group of a roboticssystem, this is usually the case.

2. Note that, as states already coincide on the base, the lines only has to be defined in G and is constant in Z. Inmost cases, it is intuitive what a suitable choice for astraight line within G should be and how it can be imple-mented easily. In less obvious cases, the exponential mapexp : g→ G can be used, where g is the Lie algebra ofG. Let d = πG( xorig)−1 πG( �v1 ( xcurr, I+v1

) ) be the differ-ence between the two states to connect. The points have tobe close enough such that d lies within the identity com-ponent of G (i.e. the image of exp), as otherwise an easy



Seiler et al. 165

connection is not possible. Then the line can be defined ass( t) := πZ ( xorig) exp( t exp−1( d) ) for a suitable pre-image

exp−1( d).

Acknowledgements

This work has been supported by the Australian Centre for FieldRobotics and the Rio Tinto Centre for Mine Automation.

References

Bloch AM (2003) Nonholonomic Mechanics and Control (Inter-disciplinary Applied Mathematics, Vol. 24). New York:Springer.

Byrd RH, Nocedal J and Waltz RA (2006) Knitro: An integratedpackage for nonlinear optimization. In Large Scale NonlinearOptimization. Berlin: Springer, pp. 35–59.

Canny JF (1988) The Complexity of Robot Motion Planning.Cambridge, MA: MIT Press.

Cheng P, Frazzoli E and LaValle SM (2008) Improving the per-formance of sampling-based motion planning with symmetry-based gap reduction. IEEE Trans Robotics 24: 488–494.

Choset H, Lynch KM, Hutchinson S, Kantor GA, Burgard W,Kavraki LE, et al. (2005) Principles of Robot Motion: Theory,Algorithms, and Implementations. Cambridge, MA: MIT Press.

Donald B, Xavier P, Canny JF and Reif J (1993) Kinodynamicmotion planning. J ACM 40: 1048–1066.

Hairer E, Lubich C and Wanner G (2006) Geometric Numer-ical Integration, 2nd edn (Springer Series in ComputationalMathematics, Vol. 31). New York: Springer.

Kanayama Y, Kimura Y, Miyazaki F and Noguchi T (1990) Astable tracking control method for an autonomous robot. InProceedings of the IEEE International Conference on Roboticsand Automation, vol. 1, pp. 384–389.

Kelly A and Nagy B (2003) Reactive nonholonomic trajectorygeneration via parametric optimal control. Int J Robotics Res22: 583–601.

Kobilarov M, Desbrun M, Marsden JE and Sukhatme GS (2007)A discrete geometric optimal control framework for systemswith symmetries. In Proceedings of Robotics: Science andSystems, Atlanta, GA, USA.

Koon W-S and Marsden JE (1995) Optimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reduction. SIAM J Control Optimiz 35: 901–929.

Lamiraux F, Bonnafous D and Lefebvre O (2004) Reactive pathdeformation for nonholonomic mobile robots. IEEE TransRobotics 20: 967–977.

Latombe J-C (1991) Robot Motion Planning. Boston, MA:Kluwer.

LaValle SM (2006) Planning Algorithms. Cambridge: CambridgeUniversity Press. Available at: http://planning.cs.uiuc.edu/.

LaValle SM and Kuffner JJ (2001) Randomized kinodynamicplanning. Int J Robotics Res 20: 378–400.

Murray RM, Li Z and Sastry SS (1994) A Mathematical Introduc-tion to Robotic Manipulation. Boca Raton, FL: CRC Press.

Nadjafikhah M and Forough A-R (2007) Galilean geometry ofmotions. Preprint arXiv:0707.3195.

Ollero A and Heredia G (1995) Stability analysis of mobile robotpath tracking. In 1995 IEEE/RSJ International Conference onIntelligent Robots and Systems, Proceedings, pp. 461–466.

Ostrowski JP (1999) Computing reduced equations for roboticsystems with constraintsand symmetries. IEEE Trans RoboticsAutomat 15: 111–123.

Park W, Reed KB and Chirikjian GS (2010) Estimation of modelparameters for steerable needles. In IEEE International Con-ference on Robotics and Automation, pp. 3703–3708.

Pham Q-C (2011) Fast trajectory correction for nonholonomicmobile robots using affine transformations. In Proceedings ofRobotics: Science and Systems, Los Angeles, CA, USA.

Seiler KM, Singh SPN and Durrant-Whyte H (2011) Using Liegroup symmetries for fast corrective motion planning. Algo-rithmic Foundations of Robotics IX, pp. 37–52.

Shewchuk JR (1994) An Introduction to the Conjugate GradientMethod Without the Agonizing Pain. Technical report, CarnegieMellon University, Pittsburgh, PA.

Webster RJ III, Cowan NJ, Chirikjian GS and Okamura AM(2004) Nonholonomic modeling of needle steering. In Pro-ceedings of the 9th International Symposium on ExperimentalRobotics.

Wong JY (2008) Theory of Ground Vehicles, 4th edition. NewYork: Wiley.

Appendix A: Lie groups

A formal introduction into the theory of Lie groups isbeyond the scope of this paper. A the reader is referred to amore exhaustive references such as Murray et al. (1994) andBloch (2003) for consideration of this topic in the roboticsdomain. The intent of this section is to give the reader amore informal survey of the properties used within thispaper.

Formally, a Lie group G is a differential manifold, thatis at the same time a group such that the group operationsare smooth. Typically G is written as a multiplicative groupwith 1 ∈ G as the neutral element. However, full gen-erality is not necessary in this context as the Lie groupsarising within the scope of this work can be representedwith G being a set of invertible square matrices using theusual matrix multiplication as group operation. The neutralelement 1 is then represented by the identity matrix.

Examples of this are SO( 2), the group of rotations in R2

written as two-dimensional rotation matrices(cos( α) − sin( α)sin( α) cos( α)

).

Similarly, SO( 3) can be written as the group of 3×3 orthog-onal matrices with determinant 1. To represent both, rota-tions and translations simultaneously, the special Euclideangroup (SE( n)) can be used. It can be represented in matrixform as homogeneous coordinates. In case of SE( 3) thisresults in 4× 4 matrices(

R t0 1

),

where R ∈ SO( 3) is a 3× 3 rotation matrix and t ∈ R3 is acolumn vector representing translation.

The dimensionality of a Lie group G is its dimensionalityas a manifold. In matrix representation that is the number of




degrees of freedom one has for choosing valid matrix coef-ficients while maintaining the matrices special properties.For example SO( 2) is one-dimensional (planar rotation),whereas SE( 3) has six dimensions (six-degree-of-freedombody).

It is important to understand that an element of a Liegroup g ∈ G can be seen as both coordinates and trans-formations. In order to view g as coordinates, one caninterpret g as representing the point that a designated ori-gin gets transformed onto by g. As a transformation, g actsby multiplication. In the case of left-multiplication as usedthroughout this work, the result of g acting on h ∈ G isthe element gh ∈ G. This dual nature of Lie groups helpseasing notation significantly.

Often there are additional (left-)multiplication laws for aLie group G to act on another manifold X . In this paper atypical situation is that X is a system’s state space and G isthe symmetry group of X . Then gx ∈ X represents the resultof transforming x by the symmetry operation represented byg. In such a setting, X can often be decomposed in the form

X = Z × G

where Z is the base space and the symmetry Lie group Gis denoted the fibre component. For this decomposition toexist, the action of G on X has to be free. That is, for allx ∈ X and g, h ∈ G it has to be true that gx = hx impliesg = h. While this is usually the case when G is a sym-metry group for a state space X , counterexamples can beconstructed easily.

Another important feature is that Lie groups come witha mechanism to elegantly formulate differential equationswithout having to worry about the choice of coordinates.Associated with a Lie group G is a Lie algebra denotedg. Loosely speaking, g is a vector space containing thetime derivatives of paths within G. That is, if a path g( t)is given, its derivative is at any given time t is representedas g−1( t) g( t). The result is similar to representing g( t) ina local frame. Using this concept, an ordinary differentialequation can be given in the form

g−1g = F( g, u) , (10)

where F is some function and u represents additionalcontrol inputs.

Applying this to a simple car for example is straightfor-ward. Typically, the equations of motion for a car containlines similar to

x = cos( θ ) v

y = sin( θ ) v

θ = ωv.

(11)

When representing x, y and θ by the Lie group SE( 2), thehomogeneous coordinates have the form

g =⎛⎝cos( θ ) − sin( θ ) x

sin( θ ) cos( θ ) y0 0 1

⎞⎠ (12)

and (11) can be rephrased as

g−1g =⎛⎝ 0 −ωv v

ωv 0 00 0 0

⎞⎠ . (13)

When integrating (10) numerically and using G in itsmatrix representation, such as in (12) and (13), care needsto be taken that the result still is in G. For example whenworking with SO( 2) or SO( 3) in matrix form, the resultof a step of numerical integration is not necessarily anorthonormal matrix any more due to numeric inaccuraciesand round-off errors. Hence, regular re-orthogonalizationand re-normalization steps might be necessary. This prob-lem can be avoided or limited by using known closed formsolutions, e.g. for exp, or carefully choosing an alternativerepresentation, e.g. a single angle θ for SO( 2) or quater-nions for SO( 3). For further information on this topic thereader is referred to Hairer et al. (2006) for example.

Appendix B: Index to Multimedia Extensions

The multimedia extension page is found at http://www.ijrr.org

Table of Multimedia Extensions

Extension Type Description

1 Video Footage of the experiment shown inFigure 13


http://www.ijrr.org/ijrr_2012/429977.htm


The International Journal of Robotics Research...

Documents

Transcript of The International Journal of Robotics Research...