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 fields. It inherits the convergence properties of harmonic
potentials. We prove impenetrability of the obstacles hull and
asymptotic stability at a final 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 flying 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-
fly 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 offline 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 Identifier (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 flow [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 artificial
potential fields [12], where each obstacle is modeled with
a repulsive potential force. While each obstacle introduces
topologically necessary critical points to a vector field [13], for
the potential field 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 find, 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 flow 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 infinite 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 define 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 first-order
partial derivative (C1smoothness). In this paper, we define:
Γ(ξ) =
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 first eigenvector
decreases to become zero on the obstacle’s hull. This cancels
the flow 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) fulfill 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 field 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 specific 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
modified 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 influence 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 deflection from the original DS with respect
to the unitary vector nf(ξ), aligned with the original DS. We
define 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 first 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 final 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 finite 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 infinite time.
D. Convergence
The magnitude (13) is by definition 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
final 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 specific case of obstacle
avoidance, a less conservative boundary condition is sufficient:
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 verified, 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 finger Robotiq-gripper). The robot is controlled at a rate
of 100 Hz. The robot is simplified 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 simplified 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 first 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 final
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 simplified
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 simplified 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 find 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 confined 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 modification 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 configuration 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 finding 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 definition 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 definition, 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 define
˜
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 first diagonal element of ˜
E(ξ)−1d
dt ˜
E(ξ)is zero, because
the derivative of the first 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 finite time, the
invariant set is only strictly outside the obstacle ξ∈Xe. We
therefore want to show that the robot stays outside the obstacle
for finite 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 finite 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 definite 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 first 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 definiteness is verified if and only if the determinant
of the symmetric part of the generalized Jacobian is positive
definite:
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 definite:
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 define 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 fulfill 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 final 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 artificial
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 field, 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.
