Content uploaded by Lukas Huber

Author content

All content in this area was uploaded by Lukas Huber on Feb 04, 2019

Content may be subject to copyright.

IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2019 1

Avoidance of Convex and Concave Obstacles

with Convergence ensured through Contraction

Lukas Huber1·Aude Billard 1·Jean-Jacques Slotine 2

Abstract—This paper presents a closed-form approach to

obstacle avoidance for multiple moving convex and star-shaped

concave obstacles. The method takes inspiration in harmonic-

potential ﬁelds. It inherits the convergence properties of harmonic

potentials. We prove impenetrability of the obstacles hull and

asymptotic stability at a ﬁnal goal location, using contraction

theory. We validate the approach in a simulated co-worker

industrial environment, with one KUKA arm engaged in a pick

and place grocery task, avoiding in real-time humans moving in

its vicinity and in simulation to drive wheel-chair robot in the

presence of moving obstacles.

Index Terms—Collision Avoidance; Optimization and Optimal

Control; Autonomous Agents

I. INTRODUCTION

ROBOTS that will work in real life environment are bound

to encounter disturbances constantly, e.g. a pedestrian

running in front of an autonomous car, a bird ﬂying in

front of a drone. The robot, which can not follow its initial

path anymore, would have to recompute a new path within

milliseconds to contour the obstacle and avoid a crash.

Control using dynamical systems (DS) is ideal to address such

situations. In contrast to classical path planning, the control

law is closed form, hence requires no re-planning, and can

ensure impenetrability of obstacles [1], [2]. They offer stability

and convergence guarantees in addition to the desired on-the-

ﬂy re-activity.

Obstacle avoidance is a classical problem in robotics and nu-

merous approaches have been proposed [3]. Recently, powerful

approaches to obstacle avoidance in cluttered environment,

ensuring global path planning with global convergence prop-

erties, have been proposed, albeit at the expenses of being

computationally expensive. These methods are usually evalu-

ated ofﬂine and remain limited to quasi static environments

[4].

Online (partial) replanning [5] or elastic-band methods de-

form locally the path and can hence be applied for dynamic

environment [6]. However, as they loose global guarantees

of convergence, hybrid algorithms must be used to switch

between global path planning and local deformation [7]. To

alleviate the computation time, recent work use customized

circuitry on a chips for faster, global sampling and evaluation

Manuscript received September 11, 2018; Revised December 05, 2018;

Accepted January 02, 2019. This paper was recommended for publication by

editor Dezhen Song upon evaluation of the associate editor and reviewers’

comments.

*This work was funded in part by the EU project Crowdbots.

1LASA Laboratory, Swiss Federal School of Technology in Lausanne -

EPFL, Switzerland. {lukas.huber;aude.billard}@epfl.ch

2Nonlinear Systems Laboratory, Massachusetts Institute of Technology,

USA. jjs@mit.edu

Digital Object Identiﬁer (DOI): see top of this page.

of all paths [8].

With improvements in hardware and computational speed,

optimization algorithm such as model predictive control have

become feasible for on-board use in dynamic path planning

and obstacle avoidance [9]. A recent approach uses power

diagrams to identify the robot’s collision free, convex neigh-

bourhood and an associated, well-known convex optimization

problem generates a continuous ﬂow [10]. This method en-

sures convergence for convex obstacles almost spherical curva-

ture. In the past years, machine learning algorithms have been

directly applied to sensor data, in order to infer data-driven

control laws [11], but later cannot ensure impenetrability.

A common approach for obstacle avoidance are artiﬁcial

potential ﬁelds [12], where each obstacle is modeled with

a repulsive potential force. While each obstacle introduces

topologically necessary critical points to a vector ﬁeld [13], for

the potential ﬁeld approach they often result in (topologically

unnecessary) minima [14]. Recent approaches use navigation

functions which transform star-shaped and trees of stars

into simpler environments where obstacles are reduced to

spheres [15]. In such convex sphere worlds the problem of

local minima could be mostly removed in free space and

convergence to the global minima is ensured for almost all

trajectories [16]. Other approaches further reduces the space

to a point-world [17] and apply navigation function based on

harmonic potentials in planar space [18].

Harmonic potential functions are particularly interesting as

they guarantee that no topologically critical points arise in free

space. However, as they are hard to ﬁnd, potential functions

are often evaluated numerically [19]. Closed-form harmonic

potential functions can be generated by approximating the

obstacles through linear panels [20]. This allows to treat

concave obstacles, but is limited to static environments [1].

In other attempts, harmonic potential function are found for

convex obstacles by using sliding mode between them [21].

This can be extended to linearly moving and rotating obstacles,

but is limited to two dimensions [1].

We extend out previous work based on closed-form equation of

harmonic potential ﬂow around simple obstacles [22]. While

the work ensures to avoid moving obstacles, it was restricted

to convex obstacles. An extension to concave obstacles using

discrete, sensor-based representation was offered in [23], [24],

but it led to the creation of a potentially inﬁnite number of

spurious attractors on the obstacle.

This paper offers an approach to avoid multiple concave

obstacles, that preserves the asymptotic stability and ensures

that no more than one single trajectory leads to a spurious

attractor.

2 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2019

II. PRELIMINARIES

ξ∈Rdis the state of the robotic system whose dynamics

is governed by an autonomous (time invariant) linear system

with a single attractor ξaof the form:

f(ξ) = −(ξ−ξa)(1)

A. Obstacle Description

As [22], we deﬁne for each obstacle a continuous distance

function Γ(ξ):Rd\Xi7→ R≥1, which allows to distinguish

three regions:

Exterior points: Xe={ξ∈Rd:Γ(ξ)>1}

Boundary points: Xb={ξ∈Rd:Γ(ξ) = 1}(2)

Interior points: Xi={ξ∈Rd\(Xe∪Xb)}

By construction Γ(·)increases monotonically with increasing

distance from the center ξcand has a continuous ﬁrst-order

partial derivative (C1smoothness). In this paper, we deﬁne:

Γ(ξ) =

d

∑

i=1

(kξi−ξc

ik/R(ξ))2pwith p∈N+(3)

with R(ξ)the distance from a reference point ξrwithin the

obstacle to the surface (Γ(ξ) = 1) in direction r(ξ)(Fig. 3a).

III. OBS TACLE AVOIDA NC E ALGORITHM

Real-time obstacle avoidance is obtained by applying a

dynamic modulation matrix to the original DS given in (1):

˙

ξ=M(ξ)f(ξ)with M(ξ) = E(ξ)D(ξ)E(ξ)−1(4)

A local modulation matrix is advantageous as it conserves

existing extrema, such as a minimum in the form of an attractor

and does not introduce new extrema as long as M(·)has full

rank [25]. It yields a closed form solution and the application

of the matrix on the system is computationally cheap.

The modulation matrix M(·)is composed of a basis matrix:

E(ξ) = [r(ξ)e1(ξ).. ed−1(ξ)],r(ξ) = ξ−ξr

kξ−ξrk(5)

and the tangents e(·)(ξ)form a d−1 dimensional orthonormal

basis to the gradient of the distance function dΓ(ξ)/dξ. The

reference direction r(ξ)is based on a reference point situated

inside the obstacle (Fig. 1a). This departs from [22] and is key

to avoid concave obstacles as we will see in Sec. III-C.

The associated eigenvalue matrix D(ξ)stretches and com-

presses the dynamics along the directions e-r-system. This

diagonal matrix allows to modify the reference r(ξ)and

tangent ei(ξ)directions individually (Fig. 1b):

D(ξ) = diag(λr(ξ),λe(ξ),...,λe(ξ)) (6)

the eigenvalues λ(·)(ξ)determine the amount of stretching in

each direction.

A. Eigenvalues

As in [22], the eigenvalue associated to the ﬁrst eigenvector

decreases to become zero on the obstacle’s hull. This cancels

the ﬂow in the direction of the obstacle and ensures that the

robot does not penetrate the surface. The eigenvalues along the

tangent direction increases the speed by the same magnitude

in each direction. This allows the robot to move around the

obstacle. Note that the eigenvalues can be set so as to ensure

that the magnitude of the velocity is preserved in certain

direction as required for the application. We have:

0≤λr(ξ)≤1λe(ξ)≥1 (7)

λr(ξ|Γ(ξ) = 1) = 0 argmax

Γ(ξ)

λe(ξ) = 1 lim

Γ(ξ)→∞λ(·)(ξ) = 1

f(ξ)ξ

r(ξ)

e(ξ)

Γ(ξ)=1

fr(ξ)

fe(ξ)

ξaξr

Γ(ξ)=2 Γ(ξ)=3

(a) Initial System

f(ξ)

ξξ

ξr

ξa

ξe

ξr

(b) Decomposition

Fig. 1: The initial, linear system f(ξ)is decomposed in tangent

e(ξ)and reference direction r(ξ)(a). The individual stretching along

tangents and compression in reference direction allow to safely avoid

the obstacle with ˙

ξ(b).

(a) No reference (b) Reference center (c) Reference top

0

2

4

6

8

10

speed

Fig. 2: Using an orthonormal basis matrix E(ξ)as in [22], a

local minimum might occur on the surface of the obstacle (a). The

placement of the reference point ξr∈Ximarked as ’+’ in (b,c)

guides the modulated DS around the obstacle.

Uising the existence of Γ(ξ)see (2), that measures the distance

to the obstacle’s surface, we can set:

λr(ξ) = 1−1/Γ(ξ)λe(ξ) = 1+1/Γ(ξ)(8)

B. Reference Point

The reference point ξrintroduced in (5) is constraint by

the fact that the basis E(ξ)needs to be invertible, i.e. have

full rank. This is the case if the reference direction r(ξ)is

linearly independent of the tangents e(·)(ξ)at any position.

This is ensured for a reference point within a convex obstacle

ξr∈Xi. The reference point can be the geometric center

ξr=ξc, but any point within the obstacle is valid (Fig. 2).

Theorem 1 Consider an obstacle in Rdwith boundary Γ(ξ) =

1with respect to a reference point inside the obstacle ξr∈Xi

as given in (2). Any trajectory {ξ}t, that starts outside the

obstacle, i.e. Γ({ξ}0)≥1and evolves according to (4), will

never penetrate the obstacle, i.e. Γ({ξ}t)≥1,t=0..∞.Proof:

see Appendix A.

C. Concave Obstacles

The shape of the obstacle is also constraint by the condition

of E(ξ)having full rank from (4). Convex and concave

obstacles for which there exist a reference point ξrinside the

obstacle, from which all rays cross the boundary only once.

(Fig. 3a) fulﬁll this condition. These obstacles are referred to

as star-shaped and are further discussed in [15].

Many obstacles which we handle in daily life are not convex,

but star-shaped, e.g. bottles, laptops, books. For more general

concave obstacles, the hull can be extended based on a

reference point ξrto make it star-shaped (Fig. 3b).

Moreover, concave obstacles can also be created by contact of

several convex obstacles (Sec. IV-B).

D. Convergence to Attractor

Any smooth vector ﬁeld in a sphere world has at least as

many topologically critical points as obstacles [13], which are

HUBER et al.: OBSTACLE AVOIDANCE USING DYNAMICAL SYSTEMS 3

(a) Star shape (b) Robot hull (c) Flow around robot

Fig. 3: Astar-shaped obstacle has one speciﬁc radius R(ξ)in

each direction φ(ξ)(a). For an obstacle, e.g. a robot arm, its hull

might have to be extended (yellow) to obtain a star-shape (b) before

evaluating the modulated DS (c). (The modulated DS is evaluated

by representing the robot with two separate convex obstacles with a

common reference point ξr.)

ideally saddle points. For a linear DS f(ξ)as in initial motion

as in (1), there is a saddle point trajectory Xsbehind the

obstacle’s reference point ξr(red line in Fig. 1b):

Xs⊂Rd={ξ∈Rd:f(ξ)kr(ξ),kξ−ξak>kξ−ξrk} (9)

The saddle point results, because the trajectory is pointing in

direction of the reference point.. Applying the decomposition

with the basis matrix E(ξ)as given in (4) results in zero mag-

nitude in tangent directions and therefore a straight trajectory.

Theorem 2 Consider a time invariant, linear DS f(ξ)with a

single attractor at ξaas in (1). The DS is modulated according

to (4) around an obstacle with boundary Γ(ξ) = 1. Any motion

{ξ}t,t=0..∞that starts outside the obstacle and lies not on

the saddle point trajectory i.e. {{ξ}0∈Rd\Xs:Γ({ξ}0)>

1}converges to the attractor, i.e. {ξ}t→ξa,t→∞.Proof:

see Appendix B.

E. Leveraging Heuristics

1) Magnitude: Applying the modulation on the DS results

in change of magnitude along the different basis directions

(Fig. 2). While there is a decrease along the reference direc-

tion, the increase in velocity along the tangent direction is

bounded to max(λe(ξ)) = 2). As the basis matrix E(ξ)is not

orthogonal, the angle ε(ξ)between r(ξ)and the tangent plane

(Fig. 9) is less than 90 degrees. Therefore, the amplitude of

the DS is upper bounded by |ξmax|:

|˙

ξ|<|˙

ξmax|=|f(ξ)|

cosε(ξ)max(λe(ξ)) = 2|f(ξ)|

cosε(ξ)(10)

The maximum magnitude |˙

ξmax|occurs on the surface of the

obstacle (Γ(ξ) = 1) and for an initial DS f(ξ)which points in

a direction by ε(ξ)inclined towards the normal n(ξ)opposite

the reference direction r(ξ)(Fig. 2).1

2) Direction: In the present approach, the DS follows the

border closely behind the obstacle (Fig. 2a); an alternative is to

move straight towards the attractor behind and obstacle [22].

IV. MULT IP LE OB STACLES

In the presence of multiple obstacles, the nominal DS is

modiﬁed by taking the weighted mean of the modulated DS

˙

ξocreated by each obstacle o=1..No; separately for the

magnitude k˙

ξokand direction n˙

ξo(Fig. 4).2The modulated

DS for each obstacle ois given as ˙

ξo=k˙

ξkon˙

ξo. To balance

1This upper bound is needed as not to exceed the hardware limit.

2The element-wise (of each Cartesian vector element) weighted mean of

the modulated DS ˙

ξofor each obstacle may create stationary points.

(a) Simulation wheelchair (b) 2D model

Fig. 4: The wheelchair (orange) tries to avoid a human crowd

represented by circular obstacles (a). In order to account for the

wheelchair’s geometry a margin around the obstacle (dashed line)

is added (b).

the effect of each obstacle and to ensure that one the boundary

of each obstacle the inﬂuence of the other obstacles vanish,

we enforce that :

No

∑

k=1

wo(ξ) = 1 and wo(ξ∈Xb,ˆo) = (1o=ˆo

0o6=ˆo(11)

Where Nois the number of obstacles, and ˆodenotes the

boundary of obstacle o.

In the experiments, we set the weight woto be inversely

proportional to the distance measure Γo(ξ)−1 (notice that

each obstacle can have its own distance measure Γo):

wo(ξ) = ∏No

i6=oΓi(ξ)−1

∑No

k=1∏No

i6=k(Γi(ξ)−1)o=1..No(12)

The magnitude is evaluated by the weighted mean:

k˙

¯

ξk=

No

∑

o=1

wok˙

ξok(13)

A. Directional interpolation

We compute the deﬂection from the original DS with respect

to the unitary vector nf(ξ), aligned with the original DS. We

deﬁne the function κ(·)∈Rd−1that projects the modulated DS

from each obstacle onto a (d−1)-dimensional hyper-sphere

with radius π(Fig. 5). 3

κ(˙

ξo,ξ) = arccosn˙

ξo

1hˆ

nξo

2.. ˆ

n˙

ξo

diT

∑d

i=2ˆ

n˙

ξo

i

,ˆ

nξo=RT

fn˙

ξo

The orthonormal matrix Rf(ξ)is chosen such that the

initial DS f(ξ)is aligned with the ﬁrst axis [ξ10]T=

Rf(ξ)Tf(ξ)with Rf(ξ) = hnf(ξ)ef

1(ξ)... ef

d−1(ξ)i. The

vectors ef

(·)(ξ)are chosen so as to form an orthonormal basis.

The weighted mean is evaluated in this κ-space (Fig. 5b):

¯

κ(ξ) =

No

∑

o=1

wo(ξ)κo(˙

ξ,ξ)(14)

The direction vector of the modulated DS ˙

¯

ξis then expressed

back in the original space:

¯

n(ξ) = Rf(ξ)hcosk¯

κ(ξ)ksink¯

κ(ξ)k

k¯

κ(ξ)k¯

κ(ξ)iT

(15)

With (13), the ﬁnal velocity is evaluated as:

˙

¯

ξ=¯

n(ξ)k˙

¯

ξk(16)

B. Intersecting Obstacles

In the case of intersecting obstacles, the above algorithm is

applicable, too, but convergence only occurs if there exists one

3In the two-dimensional case, this hyper-sphere is a line which represents

the angle between the initial DS f(ξ)and the modulated DS ˙

ξk. It has a

magnitude strictly smaller than π.

4 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2019

𝜿ሶ

𝝃𝟐, 𝛏

𝛏

ሶ

𝝃𝟏

ሶ

𝝃𝟐

ሶ

𝝃𝟑

ሶ

ത

𝝃𝝃𝒓,𝟏

𝝃𝒓,𝟐

𝝃𝒓,𝟑

𝜿ሶ

𝝃𝟐, 𝛏

𝜿ሶ

𝝃𝟏, 𝛏 𝑓 𝜉

(a) Three obstacles (b) κ-space

Fig. 5: The velocity is evaluated separately for each of the obstacles

in three-dimensional space (a). The directions are transformed to κ-

space (b) where the weighted mean, ¯

κ, is obtained.

(a) Concave regions (b) No Common ξr

Fig. 6: Several intersecting convex obstacles can be avoided, if one

common region exists obstacles (a). No global convergence can be

observed otherwise and convergence to a local minimum (yellow)

occurs (b). A star-shape hull can be created to exit those concave

regions as in Fig. 3.

common reference point ξr,o(Sec. III-B) among all obstacles.

Hence, there must exist a common region. This is always

the case for two intersecting obstacles. For more intersecting

obstacles it happens in special cases (Fig. 6). The convex

obstacles must form a star-shape as a group (see concave

obstacle in Sec. III-C).

Furthermore, if several obstacles share one reference point ξr,

there exists only one common, saddle point trajectory.

Theorem 3 Consider a time invariant, linear DS f(ξ)with

a single attractor at ξaas in (1). The DS is modulated

according to (16) around No>1obstacles with boundaries

Γo(ξ) = 1,o=1..No, which share one common reference

point ¯

ξr. Any motion {ξ}t,t=0..∞that starts outside the

obstacles and lies not on the saddle point trajectory i.e.

{{ξ}0∈Rd\Xs:Γ({ξ}0)>1}converges to the attractor,

i.e. {ξ}t→ξa,t→∞.Proof: see Appendix C.

C. Impenetrability

On the surface of an obstacle ˆo(Γo(ξ∈Xb,ˆo) = 1) the

corresponding weight is wˆo=1, whereas for all other obstacles

wo=0,o6=ˆo. Therefore, the interpolation of the velocities

is equal to: ˙

¯

ξ=˙

ξo. Impenetrability follows immediately from

the impenetrability for the single obstacle in App. A.

Note that there is a singularity on the obstacles’ boundary,

where two surfaces intersect. This does not lead to penetration

of the obstacle’s boundary, since according to App. B4 no

trajectory reaches the surface in ﬁnite time. Therefore, either

the robot avoids this concave region and converges to the

attractor, or it converges to this saddle point at the singularity,

but only reaches the surface in inﬁnite time.

D. Convergence

The magnitude (13) is by deﬁnition larger or equal to zero.

Zero velocity is only obtained if the product of the weight

wo(ξ)and magnitude are zero for all obstacles. This ensures

that no new stationary points (|˙

¯

ξ|=0) are created.

V. MOV IN G OBS TACL ES

For moving obstacles the modulation is performed in the

obstacle reference frame, and then transformed to the inertial

frame [23]:

˙

ξ=M(ξ)f(ξ)−˙

˜

ξ+˙

˜

ξwith ˙

˜

ξ=˙

ξL,o+˙

ξR,o×˜

ξ(17)

with linear and angular velocity of the obstacle with respect

to its center point ˙

ξL,oand ˙

ξR,o, respectively and the relative

position ˜

ξ=ξ−ξc. Avoiding moving obstacles is not a pure

modulation of the DS in form of a matrix multiplication

anymore, hence topographically critical points, including the

attractor, can be displaced.

A. Relative Velocity

From (17), the DS on the surface a moving obstacle has

a normal component which is equivalent to the obstacles

velocity in this direction. As a result a robot is pulled along

with a passing obstacle, e.g. a human. It might be undesired,

because it creates unnecessary change of the initial DS and

it might result in critical situations if the robot abruptly

changes direction. This effect can be reduced by considering

the obstacle’s velocity in the normal direction n(ξ)only if it

is greater than zero: ˙

˜

ξo

n=max0,˙

˜

ξo

n.

B. Forced Attraction

The convergence to the attractor ξacan be enforced with

a moving obstacle, by treating the region around the attractor

similarly to a region around an obstacle, but with modulation

matrix Ms(ξ) = I. The direction and magnitude of the initial

DS are interpolated similarly to the case of several obstacles

(Sec. IV).

The weighted sum includes the desired stationary points (·)s

for s=1..Nsand obstacles (·)ofor o=1..No. This gives the

ﬁnal magnitude:

k˙

¯

ξk=

No

∑

i=1

wo

i(ξ)k˙

ξok+

Ns

∑

j=1

ws

j(ξ)kf(ξ)k,∑

i

wo

i+∑

j

ws

j=1

where the weights are chosen inverse proportional a distance

measure to the corresponding feature.

C. Impenetrability

The Neumann Boundary condition from (20) is not en-

sured on the boundary of obstacle o, as the present obstacle

avoidance method is only evaluated partially in the moving

frame (Sec. V-A). While this ensures that no trajectory ever

penetrates the boundary Xb,o, in the speciﬁc case of obstacle

avoidance, a less conservative boundary condition is sufﬁcient:

any point starting outside of the obstacle is desired to stay there

at all time, but no such restriction is required for points inside

the boundary as this space is never reached:

˙

˜

ξoT·n(ξ)≥0∀ξ∈Xb,o(18)

with the normal on the surface n(ξ)pointing away from the

obstacle. The condition of (Sec V-A) ensures this inequality.

Furthermore, the forced attraction of Sec. V-B has no impact

on impenetrability as long as it is done around a local

HUBER et al.: OBSTACLE AVOIDANCE USING DYNAMICAL SYSTEMS 5

minima, because on the surface of an obstacle the weight of

a stationary point is zero unless it is on the boundary of the

obstacle: ws=0,if ξa,s/∈Xb. If the stationary point is on

obstacle’s boundary, the velocity is zero as a result of the

stationary condition of the attractor and the boundary condition

is veriﬁed, too.

VI. EMPIRICAL VALIDATION

The performance of the proposed framework is evaluated

on a robotic arm platform (7 DOF KUKA LWR 4+ with a

2 ﬁnger Robotiq-gripper). The robot is controlled at a rate

of 100 Hz. The robot is simpliﬁed to a sphere shape with

center at the end effector (Fig. 7b), as this is the region with

the highest probability for collision. The output, the desired

velocity of the end effector, is converted to joint torque using

impedance controller and inverted kinematics described in

[26]. The damping of the controller is chosen high enough

to have the robot accurately follow the desired trajectory.

The orientation of the end-effector is kept pointing towards the

ground. The robot is simpliﬁed to have a spherical geometry,

with center at the end-effector. The position and orientation

of moving obstacles are captured by an Optitrack system,

while the geometry of the obstacles is determined manually.

A safety margin of the radius of the robot model is added to

each obstacle, because the obstacle avoidance method ensures

impenetrability only for a point robot.

A. Scenario 1: Pick-and-place task

The ﬁrst part of the empirical validation consists of a

task encoded by three consecutive, linear DS, with switching

occurring as soon as the end effector reaches an attractor

(Fig. 7).

A) Pick: Objects (shopping groceries) are arriving on a con-

veyer belt, where the gripper is picking them up at a constant

location.

B) Place: The objects are placed at their goal position, i.e. in

a concave basket.

C) Via-point: To ensure a close to vertical descend onto the

pick location, the robot passes by the via point.

The initial motion of three consecutive, linear DS (blue arrow

in Fig. 7a) are modulated (red) to avoid the obstacles. The

large obstacle (two orange ellipsoid) on the right represents

the bottom and left wall of the basket. The small obstacle on

the left is a virtual-object, the purpose of which it is to guide

the motion. It but modulates the DS such that it has a small

vertical motion after the picking-action (Fig. 7a), this avoids

sliding along the conveyer belt. Note, that this virtual obstacle

has the same purpose for the placing-motion as the via-point

for the picking-motion, but the resulting path with using a

virtual obstacle is smoother.

During the validation, the robot picks at a constant location

on the conveyer belt incoming objects varying in weight and

form (e.g. tea bag package at 50 grams, quick-notes packages

at 300 grams). Furthermore, the position of the robot can get

disturbed by applying an external force on the end effector.

The robot is safely able to pick up the obstacles from the

conveyer belt and place them in the basket without collision.

(a) Placing (b) Avoidance below

Fig. 7: The pick-and-place is a sequence of three linear DS (blue

arrow) which are modulated to avoid the obstacles to obtain the ﬁnal

DS (red arrow) in (a). The robot path is adapted in real-time in

presence of new obstacles (b). The model of the robot is simpliﬁed

to be spherical around the end effector (red sphere).

The disturbance leads to a change in path but has no effect on

the success of the execution.

B. Scenario 2: human interference in robot task

In the second scenario, a human is interacting with the

robot, as the human is helping the robot on the conveyer belt

during the packing task (e.g. handling complicate or delicate

obstacles). The robot has full knowledge of the position of the

human forearms. Due to the slow motion of the arms and the

high update rate, they are simpliﬁed to be quasi static.

The forearms are modeled as ellipsoids with a large hull to

account for the dimensions of the end effector. The forearms

can intersect with the other obstacles or each other and create

concave forms as they move freely in space. The base task is

the same pick-and-place task as in Scenario 1.

The present method is able to ﬁnd a safe path to execute

the pick-and-place task in real-time. However, the trajectory

vary greatly depending on the newly introduced obstacle (the

forearm). The algorithm is also able to avoid the concave

region which is created, if the two obstacles representing the

arms intersect.

VII. DISCUSSION AND FUTURE WOR K

The algorithm depends on choosing well the reference point

inside the obstacle, as this shapes the modulation. We set

conditions on the set of feasible reference points but left it

to practitioners to decide which is most suitable for their

application. The choice of this reference point can become

challenging if the modulation matrix must vary in time.

A. Models of the Robot and the World

The algorithm was evaluated with prior full knowledge of

the obstacle form, and the position and velocity estimate is

assumed being error free. Future work should investigate the

effect on uncertainties in the robot’s state as well as the robot

surface being learned based on sensor data.

All control inputs and proofs are based on the assumption

of a point-shaped, zero-mass robot. While the obstacle hull

can be expanded to account for the robots dimensions, this

introduces constraint on the movement of the robot as well as

the positions of the obstacle. Furthermore, the velocity based

controller does not take into account the inertia of the robot.

This might lead to collision in conﬁned spaces and at higher

speeds, the controller needs to be extended to includes the

robot’s dynamics for the application.

6 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2019

B. Restrictions on Dynamical Systems and Obstacles

The present algorithm needs all obstacles in star-shape, for

this modiﬁcation of the robot hull might be required. Star-

shapes include any form created by two convex, intersecting

obstacles, as well as being able to model complex obstacles

relatively close. On the other hand, the algorithm cannot model

a human or even an arm-robot itself closely, as the high

number of limbs and conﬁguration most the time exceeds the

properties of being star-shaped. Hence, an extension of the

algorithm to more general concave obstacles is desired.

In this report, we focused on input DS which has a linear

form. While this is a good choice for ﬁnding a direct path to

a goal position in the presence of obstacles, in many task may

require more complex input systems, e.g. polishing or painting

of surface. Future work should extend the present algorithm

to nonlinear DS. (This might be an algorithm not based on a

matrix modulation.)

C. Optimal Path

The obstacle avoidance happens in a two dimensional, linear

plane, no matter the number of dimensions of the space

(Sec. B). This is often not the optimal case, e.g. for the case

of a long ellipsoid (such as a forearm), where the modulation

around the longest axis is equally likely as around the shorter

ones. The present algorithm should extended to optimize the

path by leveraging heuristics based on different cost metrics

(e.g. shortest path, least acceleration).

Furthermore, the combination of the present method with

global path planning such as RRT should be explored, similar

to [5], [7].

VIII. CONCLUSION

While there certainly exist faster algorithms, or ones which

can handle more complex spaces; the present method is

a simple and fast, closed-form method, which is able to

avoid relatively complex environments while still ensuring

convergence. Furthermore, it is general enough to be applied

to platforms with different sensors, movement and dimensions.

Hence it has a large range of applications, ranging from robot

arms along conveyer belts, which could work interactively

with humans, but also autonomous vehicles which continue the

desired motion rather than stop in the presence of obstacles,

such as autonomous wheel chairs in highly populated areas.

APPENDIX

A. Proof of Theorem 1

The initial DS can be written as a linear combination of

two vectors: one contained in the tangent hyper-plane fe(ξ)

and the other parallel to the reference direction fr(ξ)kr(ξ):

f(ξ) = fr(ξ) + fe(ξ) = kfr(ξ)kr(ξ) + kfe(ξ)ke(ξ)(19)

where e(ξ)is a linear combination of all tangent vectors

ei(ξ),i=1..d−1, which are described in (5).

For any point on the boundary (Γ(ξ) = 1) the modulated DS

follows from (4) and the condition in (8):

˙

ξ=λr(ξ)fr(ξ) + λt(ξ)fe(ξ) = λt(ξ)kfe(ξ)ke(ξ)∀ξ∈Xb

According to the von Neuman boundary condition, impenetra-

bility is ensured if there is no velocity in normal direction on

the surface of the obstacle:

n(ξ)T˙

ξ=n(ξ)Te(ξ)k˙

ξk=0∀ξ∈Xb(20)

ξc=ξr

ϕ(ξ)

ξa

Γ(ξ) = 2

Γ(ξ) = 1

r(ξ)

e(ξ)

R(ξ)r(ξ)

R(ξ)r(ξ)

Γ(ξ) =3

R(ξ)r(ξ)

e(ξ)

β+

β-

r(ξ)

Γ(ξ) > 0

d

dt

ξ1

ξ2

Fig. 8: Any trajectory starting outside the obstacle in the positive or

negative half-plane ends up in the invariant cone-region, purple or

red, respectively.

since per deﬁnition the tangent hyper-plane is orthogonal to

n(ξ) = dΓ(ξ)/dξ.

B. Proof of Theorem 2

The proof of the algorithm of Sec. II consists of four steps:

1. Reduction of d-dimensional problem to 2D (Sec. B1)

2. Proof of existence of cone-shaped invariant sets including

the attractor (blue and red cone in Fig. 8 (Sec. B2 & B3)

3. Proof that all trajectories except the saddle point line reach

this invariant set (Sec. B3 and Sec. B4)

4. Proof of convergence to the attractor through contraction in

this invariant set (Sec. B5)

1) Reduction to two dimensional problem: Let us consider

a linear, two dimensional plane spanned by the current position

ξ, the attractor ξaand the reference point inside the obstacle

ξr. By deﬁnition, the reference direction r(ξ)from (5) and

the initial DS f(ξ)from (1) are also contained in the linear,

2-dimensional reference plane plane Sr(ξ):

{ξ,ξa,ξr} ∈ Sr(ξ)⇒r(ξ),f(ξ)∈Sr(ξ)(21)

Using (19) we can write:

˙

ξ=M(ξ)f(ξ) = λr(ξ)fr(ξ) + λe(ξ)fe(ξ)(22)

Hence, the following can be concluded:

fr(ξ)∈Sr(ξ)⇒fe(ξ)∈Sr(ξ)⇒˙

ξ∈Sr(ξ)(23)

In other words, the modulated DS is parallel to the two

dimensional, linear hyper-plane. Hence, any motion starting in

linear, two dimensional hyper-plane {ξ}0∈S({ξ}0), remains

in this hyper-plane for all times, i.e. {ξ}t∈S({ξ}0)t=0..∞.

Further proofs of convergence can be conducted in two dimen-

sions, and is applicable to the d-dimensional case. Without

loss of generality, for further proof the attractor is placed at

the origin ξa=0and the reference point of the obstacle is

ξr= [d10], with d1>0 (Fig. 8).

2) Region with Movement Away from the Obstacle: We

want to show that there exists a region, where the DS is

moving away from the obstacle, i.e. dΓ(ξ)dt >0. Close to

the obstacle with Γ(ξ)−1<< 1, following equality for any

level function Γ(ξ)given in (2) holds Γ(ξ)R(ξ)r=ξ−ξr,

with R(ξ)∈R≥0the distance to the center to the surface

of the obstacle in reference direction (Fig. 8). We deﬁne

˜

E(ξ)=[R(ξ)r e]where e(ξ)from (5) is perpendicular to

dΓ(ξ)/dξ, to express the change of the level function close

HUBER et al.: OBSTACLE AVOIDANCE USING DYNAMICAL SYSTEMS 7

to the surface:

dΓ

dt 0T=d

dt ˜

E(ξ)−1(ξ−ξr)

=−˜

E(ξ)−1d

dt

˜

E(ξ)˜

E(ξ)−1(ξ−ξr) + ˜

E(ξ)−1M(ξ)ξ

=−0(·)

(·) (·)(·)

0−diag(1/R,1)D E−1ξ

The ﬁrst diagonal element of ˜

E(ξ)−1d

dt ˜

E(ξ)is zero, because

the derivative of the ﬁrst row, the vector to the surface, is

parallel to the tangent, i.e. d

dt R(ξ)r(ξ)ke(ξ). Furthermore,

the second element of ˜

E(ξ)−1(ξ−ξr)is zero, since ξ−ξrk

r(ξ). If the position of the robot expressed in r-e-basis is

negative: E−1ξr=ξr<0⇒d

dt Γ(ξ) = −R(ξ)λr(ξ)ξr>0,

and the DS moves away from the obstacle (purple triangle

region in Fig. 8). This is intuitively, the region where the initial

DS is already moving away from the robot: f(ξ)T·d

dξΓ(ξ)<0.

3) Convergence to Cone Region: In order for any trajectory

expect for the saddle point trajectory XSgiven in (9) to

converge to the cone region, the reference angle φ(ξ) =

arctan(ξ2/(ξ1−d1)) must decrease, where d1>0 corresponds

to the position of the reference point on the ξ1-axis. This time

derivative is evaluated with (4) as:

˙

φ(ξ) = ˜

ξ×f(ξ)/k˜

ξk2=λe(ξ)d1ξ2/k˜

ξk2(24)

and ˜

ξ=ξ−[d10]. As the eigenvalue λe(ξ)is larger than zero,

(24) is positive for ξ2>0, and negative on the other half plane.

This leads to two deductions. Firstly, any point starting outside

the obstacle and not on the saddle point, reaches a cone region

with |π−φ(ξ)| ≤ β≤min(β+,β−)with β>0. Secondly, any

cone region outside the obstacle which contains the attractor

ξaand has its center at ξris invariant.

4) Staying Outside the Obstacle for Finite Time: While any

trajectory ends up within the cone-boundary in ﬁnite time, the

invariant set is only strictly outside the obstacle ξ∈Xe. We

therefore want to show that the robot stays outside the obstacle

for ﬁnite time.

Components in direction of the n(ξ) = dΓ/dξare referred to

as (·)n. Moreover, the velocity is bounded in a real system,

with the maximum velocity in normal direction vn. The time

to transit between arbitrary levels Γ0and Γ1can be evaluated

with (7) as:

tb(ξ) = ZΓ1

Γ0

dΓ

˙

ξn

=ZΓ1

Γ0

dΓ

λr(ξ)fn(ξ)=ZΓ1

Γ0

1

fn(ξ)

Γ

Γ−1dΓ

≥1

vn

[Γ0+log(Γ0−1)−Γ1−log(Γ1−1)] (25)

Starting outside the obstacle Γ0>1, and having the goal on

the surface Γ1=1, the time results as: limΓ1→1tb(ξ)→∞.

From this it follows, that any point starting outside the obstacle

does not reach the surface in ﬁnite time.

5) Contraction Analysis: The modulated dynamical system

from (4) restated as a function of g(·):

˙

ξ=g(ξ,ξ) = M(ξ)f(ξ) = M(ξ)ξ(26)

Using Partial Contraction Theory [27], the virtual system is

chosen to as a function of the new variable γ∈Rd:

˙

γ=g(γ,ξ) = −M(ξ)γ(27)

If the system g(·)is contracting with respect to γ, and it has

the two particular solutions γ=ξand γ=ξa=0. It follows

from [27] that the ξ-system exponentially tends to ξa=0.

Hence, we need to show that the system is contracting [28]

with respect to γ. A possible contraction metric is P(ξ) =

e(ξ(t+dt))

e(ξ(t))

e'(ξ)

r(ξ(t))

r(ξ(t+dt))

r'(ξ)

r'(ξ)wr(ξ)r(ξ)

we(ξ)e(ξ)e'(ξ)

ϕ(ξ)

Ɛ(ξ)

Ɛ(ξ)

Fig. 9: The derivative of the basis matrix E(ξ)and the partial

transformation ˆ

E(ξ)for ξ2>0.

Θ(ξ)TΘ(ξ)with:

Θ(ξ) = ˆ

E(ξ)−1=wr(ξ)r(ξ)we(ξ)e(ξ)−1(28)

where Θ(ξ)is a uniformly invertible square matrix based on

(5). The system is contracting with respect to the metric P(ξ),

if the symmetric part of the generalized Jacobian Fsym =F+

FTis negative deﬁnite with:

F=d

dt Θ Θ−1+Θ∂g(γ,ξ)

∂ γ Θ−1(29)

=−ˆ

E−1(ξ)d

dt

ˆ

E(ξ)−ˆ

E(ξ)−1E(ξ)D(ξ)E(ξ)−1ˆ

E(ξ)

where ˆ

E(ξ)−1E(ξ) = diag(wr(ξ),we(ξ)). Hence the second

therm is −diag(λr(ξ),λe(ξ)). The rows of the ﬁrst therm ˙

r

and ˙

eare evaluated as:

ˆ

E−1d

dt

ˆ

E=ˆ

E−1˙wrr+wr

d

dt r˙wee+we

d

dt e(30)

="˙wr

wr− k˙

rk1

tanε

we

wrk˙

rksign(eT·˙

r)1

sinε

wr

wek˙

eksign(rT·˙

e)1

sinε

˙we

we− k˙

ek1

tanε#

with cosε(ξ) = r(ξ)T·e(ξ),˙

r=d

dt r(ξ), and ˙

e=d

dt e(ξ)as

can be seen in Fig. 9. Furthermore, under the condition that

ˆ

E(ξ)has full rank (Sec. III-B) we have ε∈]0,π[.

Negative deﬁniteness is veriﬁed if and only if the determinant

of the symmetric part of the generalized Jacobian is positive

deﬁnite:

det(Fsym) = ˙wr

wr

−k˙

rk

tanε+λr ˙we

we

−k˙

ek

tanε+λe(31)

−1

4sin2εwe

wr

k˙

rksign(eT·˙

r) + wr

we

k˙

eksign(rT·˙

e)2

>0

and its trace is negative deﬁnite:

tra(Fsym) = −˙wr

wr

−k˙

rk

tanε+λr−˙we

we

−k˙

ek

tanε+λe<0

where (·)rand (·)erefer to the direction r(ξ)and e(ξ),

respectively. We deﬁne the partial derivative with respect to

φas (·)0=∂(·)/∂ φ . One has kr0k=1 and sign(eT·r0) = 1

(Fig. 9). Conversely, the dot product (rT·e0)is positive only

in a strictly concave region. Moreover, from (8) we have

λr(ξ)≥0 and λe(ξ)≥1. For ξ2>0, we rewrite the above

inequalities:

w0

r(φ)>wr/tanεw0

e(φ)>weke0k/tanε−1/˙

φ

w0

r

wr

−1

tanεw0

e

we

−ke0k

tanε+1

˙

φ>we

wr+wr

weke0ksign(rTe0)2

4sin2ε

and analogously for the negative half-plane. These three con-

ditions put lower limits on w0

r(φ)and w0

e(φ).

There can be found wrand wewhich fulﬁll the conditions, i.e.

wr(φ) = we(φ) = exp ke0kmax

tan(εmin)+C|φ|(32)

8 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2019

with C>1+ke0kmax

2ksinεkmin .

Note that sinεand tan εare never equal zero, they might be

very small, and thus w0

rand w0

emight be large. However, this

does not contradict the existence of a contraction metric or the

contraction of the system.

C. Proof of Theorem 3

The modulation of the initial DS described in Sec. IV, can

be expressed as a linear combination of the each individual

modulation ˙

ξofor o=1..Noand the initial DS f(ξ):

˙

¯

ξ=h0(ξ)f(ξ) +

No

∑

o=1

ho(ξ)Mo(ξ)ξ(33)

where hi(ξ)≥0,i=0..Noare (unknown) scalar weights.

The group of obstacles share one common reference point

¯

ξr=ξr,o. Hence, the reduction of the d-dimensional system

to two dimension from Appendix B1 can be applied. The

coordinate system with ξ= [ξ1ξ2]Tis the same for all

obstacles (Fig. 9). Equation (24) can be restated for all the

individually, modulated DS by each of the Noobstacles:

˙

φo(ξ)·sign(ξ2)≥0ξ∈Xe,o,ξ26=0∀o∈1..No(34)

and a similar condition for the initial DS r(ξ)×

f(ξ)sign(ξ2)>0. It follows that the DS converges to the line

with φ=πbehind the obstacles, since the angle is always

increasing.

Furthermore, there can be found two half-cone regions with

φ∈]β+,π]and φ∈]−π,β−]with β+>0 and β−<0 respec-

tively (Fig. 9), where the modulation of object ˆodominates.

Such that the ﬁnal DS from (33) can be rewritten as a

reduced weighted sum: ˙

¯

ξ=ˆ

h0(ξ)f(ξ) + ˆ

hˆo(ξ)Mˆo(ξ)˙

ξ. Both

weights ˆ

h0(ξ)≥0 and ˆ

hˆo(ξ)≥0 are unknown. There might

be different dominant obstacles ˆofor the positive and negative

cones (blue and purple in Fig. 9). The generalized Jacobian is

evaluated similarly to (29) as:

F=ˆ

E−1(ξ)d

dt

ˆ

E(ξ)−ˆ

hˆoD+ˆ

h0(ξ)I(35)

where the metric ˆ

P(ξ)=(ˆ

E−1)Tˆ

E−1is based on the obstacle

ˆo. Since the condition in (32) are chosen conservatively, the

system is partially contracting using the same constraints for

the weights, hence the same contraction metric. It follows

that the system is exponentially converging in the presence

of multiple obstacles with one common reference point.

REFERENCES

[1] H. J. S. Feder and J.-J. Slotine, Real-time path planning using harmonic

potentials in dynamic environments, in Robotics and Automation, 1997.

Proceedings., 1997 IEEE International Conference on, IEEE, vol. 1,

1997, pp. 874881.

[2] S. M. Khansari-Zadeh and A. Billard, Learning stable nonlinear dy-

namical systems with gaussian mixture models, IEEE Transactions on

Robotics, vol. 27, no. 5, pp. 943957, 2011.

[3] H. M. Choset, S. Hutchinson, K. M. Lynch, G. Kantor, W. Burgard, L.

E. Kavraki, and S. Thrun, Principles of robot motion: theory, algorithms,

and implementation. MIT press, 2005.

[4] S. M. LaValle and J. J. Kuffner Jr, Rapidly-exploring random trees:

Progress and prospects, 2000.

[5] O. Brock and O. Khatib, Elastic strips: A framework for motion gen-

eration in human environments, The International Journal of Robotics

Research, vol. 21, no. 12, pp. 10311052, 2002.

[6] D. Ferguson, N. Kalra, and A. Stentz, Replanning with rrts, in Robotics

and Automation, 2006. ICRA 2006. Proceedings 2006 IEEE Interna-

tional Conference on, IEEE, 2006, pp. 12431248.

[7] J. Vannoy and J. Xiao, Real-time adaptive motion planning (ramp) of

mobile manipulators in dynamic environments with unforeseen changes,

IEEE Transactions on Robotics, vol. 24, no. 5, pp. 11991212, 2008.

[8] S. Murray, W. Floyd-Jones, Y. Qi, D. J. Sorin, and G. Konidaris, Robot

motion planning on a chip., in Robotics: Science and Systems, 2016.

[9] J. Ji, A. Khajepour, W. W. Melek, and Y. Huang, Path planning and track-

ing for vehicle collision avoidance based on model predic-tive control

with multiconstraints,IEEE Transactions on Vehicular Technology, vol.

66, no. 2, pp. 952964, 2017.

[10] O. Arslan and D. E. Koditschek, Exact robot navigation using power

diagrams, in Robotics and Automation (ICRA), 2016 IEEE International

Conference on, IEEE, 2016, pp. 18.

[11] J. Michels, A. Saxena, and A. Y. Ng, High speed obstacle avoidance

using monocular vision and reinforcement learning, in Proceedings of

the 22nd international conference on Machine learning, ACM, 2005, pp.

593600.

[12] O. Khatib, Real-time obstacle avoidance for manipulators and mobile

robots, The international journal of robotics research, vol. 5, no. 1, pp.

9098, 1986.

[13] D. E. Koditschek and E. Rimon, Robot navigation functions on mani-

folds with boundary, Advances in applied mathematics, vol. 11, no. 4,

pp. 412442, 1990.

[14] E. Rimon and D. E. Koditschek, The construction of analytic diffeo-

morphisms for exact robot navigation on star worlds, Transactions of

the American Mathematical Society, vol. 327, no. 1, pp. 71116, 1991.

[15] E. Rimon and D. E. Koditschek, Exact robot navigation using artiﬁcial

potential functions, IEEE Transactions on robotics and automation, vol.

8, no. 5, pp. 501518, 1992

[16] S. Paternain, D. E. Koditschek, and A. Ribeiro, Navigation functions for

convex potentials in a space with convex obstacles, IEEE Transactions

on Automatic Control, 2017.

[17] S. G. Loizou, The navigation transformation, IEEE Transactions on

Robotics, vol. 33, no. 6, pp. 15161523, 2017.

[18] S. G. Loizou, Closed form navigation functions based on har-

monic potentials, in Decision and Control and European Control

Conference(CDC-ECC), 2011 50th IEEE Conference on, IEEE, 2011,

pp. 63616366.

[19] C. I. Connolly, J. B. Burns, and R. Weiss, Path planning using laplaces

equation, in Robotics and Automation, 1990. Proceedings., 1990 IEEE

International Conference on, IEEE, 1990, pp. 21022106.

[20] J.-O. Kim and P. K. Khosla, Real-time obstacle avoidance using har-

monic potential functions, IEEE Transactions on Robotics and Automa-

tion, vol. 8, no. 3, pp. 338349, 1992.

[21] J. Guldner and V. I. Utkin, Sliding mode control for an obstacle

avoidance strategy based on an harmonic potential ﬁeld, in Decision

and Control, 1993., Proceedings of the 32nd IEEE Conference on, IEEE,

1993, pp. 424429.

[22] S. M. Khansari-Zadeh and A. Billard, A dynamical system approach

to realtime obstacle avoidance, Autonomous Robots, vol. 32, no. 4, pp.

433454, 2012.

[23] S. Khansari-Zadeh, A dynamical system-based approach to modeling

stable robot control policies via imitation learning, PhD Thesis, 2012.

[24] M. Saveriano and D. Lee, Distance based dynamical system modulation

for reactive avoidance of moving obstacles, in Robotics and Automation

(ICRA), 2014 IEEE International Conference on, IEEE, 2014, pp.

56185623.

[25] K. Kronander, M. Khansari, and A. Billard, Incremental motion learning

with locally modulated dynamical systems, Robotics and Autonomous

Systems, vol. 70, pp. 5262, 2015.

[26] K. Kronander and A. Billard, Passive interaction control with dynamical

systems, IEEE Robotics and Automation Letters, vol. 1, no. 1, pp.

106113, 2016.

[27] W. Wang and J.-J. E. Slotine, On partial contraction analysis for coupled

nonlinear oscillators, Biological cybernetics, vol. 92, no. 1, pp. 3853,

2005.

[28] W. Lohmiller and J.-J. E. Slotine, On contraction analysis for nonlinear

systems, Automatica, vol. 34, no. 6, pp. 683696, 1998.