Fast Anytime Motion Planning in Point Clouds by Interleaving Sampling and Interior Point ...

16 Pages·2017·2.04 MB·English

Fast Anytime Motion Planning in Point Clouds by
Interleaving Sampling and Interior Point Optimization
AlanKuntz,ChrisBowen,andRonAlterovitz
UniversityofNorthCarolinaatChapelHill
ChapelHill,NC27599-3175,USA
{adkuntz,cbbowen,ron}@cs.unc.edu
Abstract. Roboticmanipulatorsoperatinginunstructuredenvironmentssuchas
homesandofficesneedtoplantheirmotionsquicklywhilerelyingonreal-world
sensors,whichtypicallyproducepointclouds.Toenableintuitive,interactive,and
reactiveuserinterfaces,themotionplancomputationshouldprovidehigh-quality
solutionsquicklyandinananytimemanner,meaningthealgorithmprogressively
improvesitssolutionandcanbeinterruptedatanytimeandreturnavalidsolu-
tion.Toaddressthesechallenges,wecombinetwoparadigms:(1)asymptotically-
optimalsampling-basedmotionplanning,whichiseffectiveatprovidinganytime
solutionsbutcanstruggletoquicklyconvergetohighqualitysolutionsinhighdi-
mensionalconfigurationspaces,and(2)optimization,whichlocallyrefinespaths
quickly.Weproposetheuseofinteriorpointoptimizationforitsabilitytoperform
inananytimemannerthatguaranteesobstacleavoidanceineachiteration,andwe
provideanovellazyformulationthatefficientlyoperatesdirectlyonpointcloud
data.Ourmethoditerativelyalternatesbetweenanytimesampling-basedmotion
planning andanytime, lazyinterior pointoptimization tocompute highquality
motionplansquickly,convergingtoagloballyoptimalsolution.
1 Introduction
Robotic manipulators are entering unstructured environments, such as homes, offices,
hospitals, and restaurants, where robots need to plan motions quickly while ensuring
safetyviaobstacleavoidance.Motionplanninginsuchsettingsischallenginginpartbe-
causetherobotmustrelyonreal-worldsensorssuchaslaserscanners,RGBDsensors,
orstereoreconstruction,whichtypicallyproducepointclouds.Inaddition,enablingin-
tuitive,interactive,andreactiveuserexperiencesrequiresthattherobotgenerateplans
ofhighqualityasquicklyaspossible,withoutnecessarilyknowinginadvancethemax-
imum time allocatable to motion planning. Hence, motion planning in such settings
shouldbeimplementedasananytimealgorithm,meaningthealgorithmprogressively
improvesitssolutionandcanbeinterruptedatanytimeandreturnavalidsolution.
Toenablefast,anytime,asymptotically-optimalmotionplanninginpointclouds,we
proposetoblendtwopopularmotionplanningparadigms:(1)sampling-basedmotion
planningand(2)optimization-basedmotionplanning.Sampling-basedmotionplanners
candirectlyusepointclouddatabyincorporatingappropriatecollisiondetectionalgo-
rithms [19] and can optimize a cost metric in an asymptotically optimal manner [12].
2 AlanKuntz,ChrisBowen,andRonAlterovitz
However,sampling-basedmotionplannersinpracticecanbeslowtoconvergeandfre-
quentlyreturnpathsinfinitetimethatarefarfromoptimal,especiallyforproblemswith
higher dimensional configuration spaces [10]. In contrast, optimization-based motion
planners are often very fast [21,23,20,11], but typically do not converge to globally
optimal solutions, do not operate in an anytime manner (since, even when initialized
withafeasiblesolution,theirintermediateiterationsmayconsiderrobotconfigurations
thatareincollisionwithobstacles),andareinefficientwhenusinglargepointclouds.
Inthispaper,weintroduceanewoptimizationapproachtomotionplanningbasedon
interior point optimization [25,24], which has unique properties that enable anytime
motion planning and efficient handling of large point clouds. By integrating our new
interior point optimization formulation with a sampling-based method, our approach
quickly computes locally optimized plans in an anytime fashion, and continues to re-
finethesolutiontowardglobaloptimalityforaslongastimeallows.
Ournewmethod,InterleavedSamplingandInteriorpointoptimizationMotionPlan-
ning (ISIMP), interleaves global exploration with local optimization (see Fig. 1). The
methodstartswithglobalexplorationbybuildingagraphusinganasymptoticallyopti-
malmotionplanner,suchask-nearestProbabilisticRoadmap(PRM*)[12],untilitfinds
acollision-freepath.Itthenusesourlazyinteriorpointoptimizationformulationtore-
finethepathfoundbythesampling-basedmethod.Thealgorithmtheniteratesbetween
(1) resuming the sampling-based motion planner until a new better path is found and
(2) running interior point optimization on this new path. The sampling-based motion
planningphaseofeachiterationexploresglobally,discoveringotherhomotopyclasses
in configuration space, as well as escaping local minima in the path cost landscape.
Theinteriorpointoptimizationphaseinturnallowsthemethodtoprovideahighqual-
itylocally-optimizedmotionplanbasedonthebestpathfoundbythesampling-based
method. Either phase can be interrupted at any time and still return a valid, collision-
freeresult.Interleavingthesephasesprovideshigherqualitymotionplansearlierthan
asymptoticallyoptimalsampling-basedmotionplanningalgorithmsalone.Inthisway,
our method preserves guarantees which are not provided by most optimization-based
motionplanningalgorithms—namelycompletenessandasymptoticoptimality—allin
ananytimefashionanddesignedspecificallytoworkefficientlyonpointclouddata.
ISIMPisbasedoninteriorpointoptimization,atypeoflocaloptimizationthathas
propertiescriticaltoachievingfast,anytimemotionplanningforroboticmanipulators.
Interiorpointmethodssolveoptimizationproblemsbyonlyconsideringfeasiblepoints
in each iteration of the method. These methods typically incorporate constraints into
the objective function by adding barrier functions, continuous functions whose value
on a point increases to infinity as the point approaches the boundary of the feasible
region. These methods adjust weights on the barrier functions over time so the so-
lution converges to a locally optimal solution of the original objective function. We
applyinteriorpointoptimizationtorefiningmotionplansandshowthatitisfast(like
othermotionplanoptimizers,e.g.,[21,23,20,11,13,18])whileprovidingthreeaddi-
tionalimportantproperties.First,ourinteriorpointoptimizationimplementationisan
anytimealgorithm:eachintermediateiterationoftheoptimizationalgorithmconsiders
onlycollision-freerobotconfigurations,sotheoptimizeralwaysreturnsafeasiblemo-
tionplanevenwheninterrupted.Second,interiorpointoptimizationcanbeformulated
FastAnytimeMotionPlanninginPointClouds 3
S G S G S G
(a)SampledFirstPath (b)OptimizedFirstPath (c)SampledSecondPath
S G
(d)OptimizedSecondPath (e)SampledPathonBaxter (f)OptimizedPathonBaxter
Fig.1. ISIMP interleaves sampling-based and optimization-based motion planning. (a) ISIMP
firstperformssampling-basedmotionplanninguntilafeasiblemotionplanisfoundfromstartto
goal.(b)Itthenusesinteriorpointoptimizationtolocallyoptimizetheplan.(c)Sampling-based
motion planning resumes until a shorter plan is found. (d) The shorter plan is then optimized.
Themethoditeratesinthisfashionandcanbeinterruptedatanytimeafterstep(a)andreturn
afeasiblesolutionthatgetsbetterovertime.(e)Anexamplesampling-basedplanfoundfora
BaxterrobotinapointcloudsensedbyMicrosoftKinect.(f)Thatplanlocallyoptimized.
toefficientlyanddirectlyhandlelargepointclouds,whichweaccomplishusingalazy
evaluation of constraints. This eliminates the time-consuming process of transform-
ingthepointcloudintomorecomplexgeometricprimitivesormeshes,whichisoften
required by other optimization-based motion planners to be efficient. Third, our inte-
riorpointoptimizationisdesignedtooptimizepathlength,acommonlydesiredmetric
which is often used by sampling-based motion planners. Many existing optimization-
basedmotionplanners[21,23,20,11]aredesignedtooptimizemetricssuchassmooth-
ness,whichdonotsatisfythetriangleinequalityandhenceareincompatiblewithcost
requirementsofasymptotically-optimalsampling-basedmotionplanners.
We evaluate the efficacy of ISIMP for a Baxter robot’s manipulator arm using a
MicrosoftKinecttosensepointclouddatainaclutteredenvironment.Wedemonstrate
ISIMP’sfast,anytime,andasymptoticallyoptimalperformanceincomparisontoother
motionplannersthatonlyuseeithersamplingorlocaloptimization.
2 RelatedWork
Insampling-basedmotionplanning,agraphdatastructureisconstructedincrementally
viarandomsamplingprovidingacollision-freetreeorroadmapintherobot’sconfigu-
rationspace.Thesealgorithmsprovideprobabilisticcompleteness,i.e.,theprobability
offindingapath,ifoneexists,approachesoneasthenumberofsamplesapproachesin-
finity.ExamplesincludetheRapidly-exploringRandomTree(RRT)[16]andtheProb-
4 AlanKuntz,ChrisBowen,andRonAlterovitz
abilistic Roadmap (PRM) [13] methods. These methods have been adapted in many
ways,includingtotakeadvantageofstructureintherobot’sconfigurationspaceandby
modifyingthesamplingstrategy[6,15].
Adaptations of these algorithms can provide asymptotic optimality guarantees in
which the path cost (e.g., path length) will approach the global optimum as the num-
berofalgorithmiterationsincreases.ExamplesincludeRRT*andPRM*[12]wherethe
underlyingmotionplanninggraphiseitherrewiredorhasasymptoticallychangingcon-
nectionstrategies.Otherasymptoticallyoptimalalgorithmsgrowatreeincost-to-arrive
space[10],identifyverticeslikelytobeapartofanoptimalpath[1],orinvestigatethe
distributionsfromwhichsamplesandtrajectoriesaretaken[14].Lazycollisioncheck-
inghasbeenshowntosubstantiallyimprovethespeedofthesealgorithms[9],andin
somecases,nearoptimalitycanbeachievedwhileimprovingspeed[22,7].
Optimization-basedmotionplanningalgorithmsperformnumericaloptimizationin
a high dimensional trajectory space. Each trajectory is typically encoded as a vector
of parameters representing a sequence of robot configurations or controls. A cost can
becomputedforeachtrajectory,andthemotionplanner’sgoalistocomputeatrajec-
tory that minimizes cost. In the presence of obstacles and other constraints, the prob-
lemcanbeformulatedandsolvedasanumericaloptimizationproblem.Examplesin-
cludetakinganinitialtrajectoryandperforminggradientdescent[21],usingsequential
quadraticprogrammingwithinequalityconstraintstolocallyoptimizetrajectories[23],
and combining optimization with re-planning to account for dynamic obstacles [20].
Thesemethodstypicallyproducehighqualitypathsbutarefrequentlyunabletoescape
localminima,andassucharesubjecttoinitializationconcerns.Toavoidlocalminima,
somemethodsinjectrandomness[11,3].Thesolutionsoftheseexistingoptimization-
basedmotionplannersmayconvergetolocallyoptimalmotionplansthatincluderobot
configurations that are in collision with obstacles. This limitation can be partially cir-
cumventedthroughtechniquessuchasrestartingtheoptimizationwithmultiplediffer-
entinitialpaths(e.g.,[23]),butsuchapproachesareheuristicandprovidenoguarantee
thatacollision-freetrajectorywillbefoundingeneral.
Severalmethodsaimtobridgethegapbetweensampling-basedmethodsandopti-
mization.Somemethodsusepathsgeneratedbyglobalplannersandrefinethemusing
shortcuttingorsmoothingmethodsadaptivelyorinpostprocessing[18,8].GradienT-
RRTmovesverticestolowercostregionsusinggradientdescentduringtheconstruction
of an RRT [2]. More recently, local optimization has been used on in-collision edges
duringsampling-basedplanningtobringthemoutofcollisionandeffectivelyfindnar-
rowpassages[4].Incontrast,ourmethodisusingthelocaloptimizernottofindnarrow
passages,buttoimprovetheoverallqualityofthepathsfoundbythesampling-based
planner, while relying on the sampling-based planner’s completeness property to dis-
cover narrow passages. BiRRT-Opt [17] utilizes a bi-directional RRT to generate an
initial trajectory for trajectory optimization, demonstrating the efficacy of a collision-
freeinitialsolutionforlocaloptimization.Ourmethoddiffersinthatinteriorpointopti-
mizationprovidescollision-freeiteratesallowingittoworkinananytimefashion,and
ourmethodcontinuesbeyondasinglelocaloptimizationtoprovideglobalasymptotic
optimality.
FastAnytimeMotionPlanninginPointClouds 5
3 ProblemDefinition
LetC be theconfiguration spaceof therobot. Let q∈C represent asingle robotcon-
figurationofdimensionalityd andΠ ={q ,q ,...,q }representacontinuouspath
0 1 n−1
inconfigurationspaceinapiecewiselinearmannerbyasequenceofnconfigurations.
Suchpathsmayneedtosatisfygeneralized,user-definedinequalityconstraints.These
constraintscouldincludejointlimits,endeffectororientationrequirements,etc,where
each constraint may be represented by the inequality g(Π)≥0 for some constraint
specificfunctiong.Letthesetofallsuchuser-definedconstraintsbeJ.
In the robot’s workspace are obstacles that must be avoided, and which are being
represented by a point cloud. We consider each point in the point cloud an obstacle
andletthesetofallsuchpointsbeO.Wealsodefinetherobot’sgeometryasasetof
geometricprimitivesS.Anindividualgeometricprimitives∈Scouldberepresentedas
amesh,boundingsphere,capsule,etc.
A path is collision-free if the robot’s geometry S over each edge (q,q ),i =
i i+1
0,...,n−2, does not intersect an obstacle o ∈ O. Formally, we require a function
clearance(q,q ,s,o)whichisafunctionparameterizedbyapathedge(q,q ),
i i+1 i i+1
a geometric primitive s, and an obstacle o. This function, formally defined in Sec. 4,
is continuous and monotonically increasing, has positive value when o is not in col-
lision with s over the edge, negative value when o penetrates into s on the edge, and
0 at the boundary. A collision-free path then becomes a path for which each edge
has non-negative clearance for all o∈O and s∈S. This requirement can be repre-
sented as an additional set of inequality constraints, which we define as K, wherein
clearance(q,q ,s,o)≥0,forallo∈Oands∈S.
i i+1
Our objective is to find a collision-free path for the robot from the robot’s start
configurationq toauserspecifiedgoalconfigurationq thatsatisfiesallthecon-
start goal
straintsandminimizespathlength.Wedefinepathlengthbylength(Π),thesumof
Euclideandistancesinconfigurationspacealongthepath.WeusethesumofEuclidean
distances because it is a commonly used metric for path length and because it satis-
fiesthetriangleinequalitypropertyrequiredbyasymptoticallyoptimalsampling-based
motionplanners(unlikeothermetricssuchassumofsquareddistances).Thisoptimal
motion planning problem can be formulated as a nonlinear, constrained optimization
problem:
Π∗=argminlength(Π)
Π
Subjectto:
clearance(qi,qi+1,s,o)≥0, 0≤i<n−1, ∀o∈O, ∀s∈S (1)
g(Π)≥0, ∀g∈J
q =q
0 start
q =q
n−1 goal
whereΠ∗isanoptimalmotionplan.Tosolvethisoptimizationproblem,wepresentan
efficient,anytime,iterativealgorithminwhichthesolutionasymptoticallyapproaches
Π∗.
6 AlanKuntz,ChrisBowen,andRonAlterovitz
4 Method
ISIMPintegratesideasfrombothsampling-basedandoptimization-basedmotionplan-
ning.Themethodinterleavesinteriorpointoptimizationstepswithsampling-basedmo-
tionplanningstepstoachievefastconvergence.Thetoplevelalgorithm,Alg.1,runsin
ananytimemanner,iteratingastimeallowsandstoringthebestpathfoundasitruns.
Algorithm1:ISIMP
Input:qstart,qgoal,O,optimizationconvergencethreshold∆Π
Output:motionplanΠ∗
1 Sampling-basedmotionplannerpathcostcsamp←∞
2 Π∗←0/
3 whiletimeremainingdo
4 Πsamp←GlobalExplorationStep(O,csamp,qstart,qgoal)
5 csamp←cost(Πsamp)
6 Π∗←minCostPlan(Πsamp,Π∗)
7 Π∗←LazyAnytimeInteriorPointOptimization(Πsamp,O,∆Π,Π∗)
8 end
The first step of each iteration is the global exploration step, wherein ISIMP exe-
cutesasampling-basedmotionplanneruntilanewpathisfoundthathascostlowerthan
c ,thebestcostfoundbythesampling-basedmotionplannerupuntilthatpoint.The
samp
sampling-basedmotionplannerreturnsthenewpathΠ andupdatesc .Inthesec-
samp
ondstepofeachiteration,theinteriorpointoptimizationstep,ourmethodexecutesan
interiorpointlocaloptimizationmethodtolocallyoptimizeΠ.Thelocaloptimizercon-
stantlycomparesagainstthebestpathfound,andupdatesitifabetterpathisfounddur-
ingtheoptimization.Attheendoftheinteriorpointoptimizationitreturnsthebestpath
found. The algorithm then iterates, returning to global exploration with the sampling-
basedmotionplanner,andoptimizingbetterpathsastheyarefound.Ifthealgorithmis
interrupted,itreturnsthebestpathfoundupuntilthattime.
4.1 GlobalExplorationusingSampling-basedMotionPlanning
The global exploration step uses a sampling-based motion planner to expand a graph
untilanewpathisfoundthatisoflowercostthananypathithaspreviouslyfound.The
sampling-basedmotionplannermaintainsagraphG=(V,E),whereV isasetofver-
ticeswhichrepresentcollision-freeconfigurationsoftherobotandE isasetofedges,
whereanedgerepresentsacollision-freetransitionbetweentworobotconfigurations.
To expand the graph G, our method is designed to use an asymptotically optimal
sampling-based motion planner. Although many methods would work, we implement
ISIMPwithbothk-nearestProbabilisticRoadmap(PRM*)[12]andRRT# [1].PRM*
samplesrandomconfigurationsintherobot’sconfigurationspace,locatestheirknear-
est neighbors (where k changes as a function of the number of vertices in the graph),
FastAnytimeMotionPlanninginPointClouds 7
and attempts to connect the configurations to each of its neighbors. RRT# is a state-
of-the-artasymptoticallyoptimalsampling-basedmotionplanner,whichincrementally
buildsatreeidentifyingwhichverticesarelikelytobelongtoanoptimalpath.Ineach
globalexplorationstep,theasymptoticallyoptimalsamplingbasedmotionplannerex-
ecutesuntilitfindsacollision-freepathbetterthanthecurrentbest,atwhichpointthe
optimizationstepbegins.
4.2 LazyInteriorPointOptimization
Local optimization for motion planning is typically performed by constructing a high
dimensional vector out of the state variables of the problem to be optimized. In our
case, this would be a vector representing the motion plan we are optimizing, i.e. if
we have a path with n configurations of dimensionality d, then each motion plan can
be represented as an n×d dimensional vector. Optimization can then be viewed as
iterativelymovingthisvectorthroughitshighdimensionalspacetominimizethecost.
Inthecaseofconstrainedoptimization,thereareregionsofthisspacewhichrepresent
areaswheretheconstraintsaresatisfied(i.e.,feasibleorunconstrainedspace)andareas
wheretheconstraintsarenotsatisfied(i.e.,infeasibleorconstrainedspace).
Manyotherclassesofoptimizationmethods(suchaspenaltymethodsandsequen-
tialquadraticprogramming)allowtheirintermediatesolutionstomovethroughinfeasi-
blespace,i.e.collidewithobstaclesorviolatejointlimits,withthehopethattheeven-
tuallocallyoptimalsolutionisfeasible.Interiorpointmethods,bycontrast,requiretheir
intermediate solutions to always be feasible [25,24]. This is typically accomplished
throughtheuseofbarrierfunctions.Abarrierfunctionworksbyintroducingacontinu-
ousfunctionwhosevalueapproachesinfinityattheedgeoftheconstrainedspace.The
intermediate iterations of optimization are then influenced by the barrier functions to
avoid the constrained regions. As the optimization iterates, the width of these barrier
functionsisreducedsuchthatthesolutionisallowedtoapproachtheconstrainedspace
but not to enter it. This is the property that we leverage to create an anytime solution
inside the optimization framework. Each intermediate solution of the optimization is
alwaysacollision-freemotionplanandassuchcanbereturnedearlyifnecessary.
Webuildaninteriorpointoptimizationframeworkaroundablackboxinteriorpoint
optimizer.Theoptimizerisresponsibleforgeneratingintermediate,feasiblesolutions,
andourframeworkupdatesthestatefortheoptimizerasafunctionofthoseintermediate
solutionstoallowforfastercomputation.Detailsofourlazyinteriorpointoptimization
frameworkcanbeseeninAlg.2.
We optimize our paths with respect to path length. The collision avoidance con-
straintsareformulatedovereachedgeinthepath,i.e.thelinearinterpolationthrough
configurationspacebetweenthetwoconfigurations.Wedefineourclearancefunction
(as in equation (1)) as a function of the minimum distance between the point in the
point cloud and the geometric primitive interpolated through the workspace as deter-
minedbytherobot’sforwardkinematics.Thisformulationgeneratesalargenumberof
constraints(#ofrobotgeometricprimitives×#ofedgesinthepath×#ofpointsinthe
pointcloud).Becausetheoptimizationmustconsidereachconstraint,fewerconstraints
resultsinquickeroptimizationtimes.Inthefollowingparagraphs,wediscussournovel
8 AlanKuntz,ChrisBowen,andRonAlterovitz
Algorithm2:LazyAnytimeInteriorPointPathOptimization
Input:initialpathΠ ,obstaclesetO,convergencethreshold∆ ,bestfoundpathso
init Π
farΠ∗
Output:bestpathfoundΠ∗
1 Kenab←0/,Πˆ ←Πinit,Π←Πinit
2 whiletimeremainingdo
3 Πnext←takeInteriorPointOptimizationStep(Kenab,Π)
4 Ocollide←inCollisionPoints(Πnext,O)
5 ifOcollide(cid:54)=0/ then
6 Kenab←Kenab(cid:83)Ocollide
7 Π←Πinit
8 else
9 ifdiscontinuousConstraintDetected(Kenab,Πnext)then
10 {k,k+1}=discontinuousJacobianEdge(Kenab,Πnext)
11 Πinit←bisectEdge(Πinit,k,k+1)
12 Π←Πinit
13 else
14 ifconverged(Πnext,Πˆ,∆π)then
15 returnminCostPlan(Πnext,Πˆ,Π∗)
16 end
17 Πˆ ←minCostPlan(Πnext,Πˆ)
18 Π∗←minCostPlan(Πˆ,Π∗)
19 Π←Πnext
20 end
21 end
22 end
23 returnΠ∗
approachtoreducingthenumberofconstraintsthatareconsideredbytheoptimization
byaddingconstraintsinalazyfashion.
Improving Performance through Lazy Constraint Addition Instead of using the
entiresetofobstacle-basedinequalityconstraintsK,whichcontainsconstraintsforeach
pointinthepointcloud,weinsteaddefineasubsetK astheenabledconstraintset.
enab
Thisset,whichstartsoutempty,isaddedtoaspointsinthecloudbecomerelevantto
theoptimization(seeFig2andAlg.2lines5-7).
Westarttheoptimizationwithanemptyenabledconstraintset(Fig.2(a)).Wethen
take an optimization step, modifying the motion plan (Fig. 2(b)). At each iteration of
suchastep,wechecktherobot’spathforcollisionwiththewholepointcloud(anoper-
ationwhichiscomputationallyinexpensivecomparedwithoptimizingwitheachpoint
asaconstraint).Ifthepathisfoundtobeincollision,weidentifywhichpointsarein
collision, and add those points to the enabled constraint set, and restart the optimiza-
tion(Fig.2(c)).Inthenextiteration,theoptimizeravoidscollisionwiththepreviously
addedpoints(Fig.2(d)).Theprocessthenrepeatsuntilacollision-freeconvergenceis
achieved. In this way, only points which prove to be relevant over the course of the
FastAnytimeMotionPlanninginPointClouds 9
S G S G
(a)PRM*PathEmpyConstraintSet (b)InCollisionOptimizationIteration
S G S G
(c)PRM*PathwithAddedConstraints (d)OptimizedCollision-FreePath
Fig.2.Anexampleofthewayourmethodaddsconstraintsfromthepointcloudinalazyfashion,
usinga2Ddiskrobotinapointcloud(greypointsareun-enabledconstraintpoints)(a)Example
unoptimizedpathreturnedbythePRM*andusedtoinitializetheoptimizer.Theoptimizerstarts
outwithanemptyenabledconstraintset.(b)Duringoptimization,therobot’spathisfoundto
beincollisionwiththepointcloud(orangepoints).(c)Thein-collisionpointsofthecloudare
immediatelyaddedtotheenabledconstraintset(blackpoints)andoptimizationisrestarted.(d)
Theoptimizerconvergestoacollision-freepathusingtheenabledconstraintset,whichissmaller
thanthesetofallpoints.
optimizationaretakenintoaccountbytheoptimizer.Thisimprovesthecomputational
speedoftheoptimizationalgorithmdrastically,asinpracticerelativelyfewpointsturn
outtoberelevanttocollisiondetectioninanotherwiselargepointcloud.
Enabled Constraint Cutoff Function One of the most time consuming parts of the
optimizationiscomputingtheconstraintJacobianfortheconstraintsinK ,whichhas
enab
arowforeachconstraintandacolumnforeachstatevariable,i.e.d×(n−1)columns.
Evenifaconstraintisrelevanttotheoptimizationasawhole,andassuchincludedin
K , sometimes the point represented by the constraint is far away from the specific
enab
robot geometric primitive or edge representing an entry in the Jacobian. To leverage
this intuition and further speed up the evaluation of the constraints in K , we use
enab
a cutoff function (3) as our clearance function (see Fig. 3(a)) rather than the distance
itself,allowingtheusertosetaradius,r>0,asa“look-ahead”distance.Let
d=min dist(s,q,q ,o), (2)
i i+1
2(cid:0)d(cid:1) (cid:0)d(cid:1)<0
 r r
clearance(d,r)= (cid:0)d(cid:1)4−2(cid:0)d(cid:1)3+2(cid:0)d(cid:1) 0≤(cid:0)d(cid:1)≤1 (3)
r r r r
1 (cid:0)d(cid:1)>1,
r
10 AlanKuntz,ChrisBowen,andRonAlterovitz
wheremin dististheminimumdistancebetweenthepointoandthegeometricprim-
itiveswhensissweptalongthespacecurvedefinedbythetransitionbetweenconfig-
urations q and q , i.e. as the robot arm is moving from configuration q to q .
i i+1 i i+1
Equation 3 sends the clearance function’s derivative to zero at r, allowing us to spar-
sifytheconstraintJacobianandonlyconsiderpointsinK whicharewithindistance
enab
r from the robot’s geometry at a given time. This allows even greater speedup to the
optimization,astheconstraintsetK ,whichisalreadygreatlyreducedinsizefrom
enab
the full point cloud, is still larger than needs to be considered in many entries of the
constraintJacobian.
1
ecnarae 0 s1 qk qk+1 s1 qk qk+1 qk+2
lc
d
-1-0.5 0 0.5 1 1.5 d2 d1 d2 o 1
d o
min_dist(q, q , s,o) = d,
k k+1 1 2
r min_dist(qk, qk+1, s1,o) = d1= d2 min_dist(qk+1, qk+2, s1,o) = d1
(a)CutoffFunction (b) Non-Differentiable Mini- (c)Bisection
mumDistance
Fig.3. (a) The cutoff function used as the clearance function for our obstacle avoidance con-
straints.Thefunctionflattensoutafteracertaindistancesoastoallowustoonlyconsiderpoints
inK whicharewithinaspecifiedradiusr.(b-c)Anexamplewithamanipulator(shownin
enab
red).(b)Ifasphereboundingtherobotgeometryissweptbetweentwoconfigurationsandpro-
ducesacurvesuchasthebluecurvehere,thenwhend andd areequal,theminimumdistance
1 2
betweenspheres sweptfromq toq andpointcloudpointoisnotnecessarilydifferentiable.
1 k k+1
(c)Whensuchacaseisdetected,anintermediateconfigurationisaddedintothepathwithits
ownconstraint.Inthisway,bothd andd areusedindifferentconstraintsandarebothlocally
1 2
differentiable.
Anotherchallengeofourconstraintformulationisthattheminimumclearancebe-
tweenagivenrobotgeometricprimitivealonganedgeandapointinthepointcloudis
notguaranteedtobedifferentiable.Thisisbecausetherobot’sarmistracinganonlinear,
complexcurvethroughtheworkspace,eventhoughtheedgeislinearinconfiguration
space. This means that in some places, the closest position on that curve to an obsta-
cle point can be discontinuous as the edge is perturbed (see Fig. 3(b)), i.e., there are
multiple closest positions on the curve which are equidistant to the point cloud point.
Usingdistanceoverthenonlinearedgeallowsustoensureobstacleavoidance,butcan
causenumericalproblemsduringtheoptimization.Toaddressthisissue,whenweare
evaluatingthederivativeofaconstraintandfindittobediscontinuousoveranedge,we
bisectthatedgeandstarttheoptimizationanew(seeFig.3(c),andAlg.2lines9-12).In
thisway,afterenoughbisectionsthediscontinuityisavoided.Inpractice,weobserved
relativelyfewbisections.