ArticlePDF Available

Avoidance of Convex and Concave Obstacles With Convergence Ensured Through Contraction


Abstract and Figures

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 obstacle's 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.
Content may be subject to copyright.
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
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
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’
*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}
2Nonlinear Systems Laboratory, Massachusetts Institute of Technology,
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
ξ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→ R1, which allows to distinguish
three regions:
Exterior points: Xe={ξRd:Γ(ξ)>1}
Boundary points: Xb={ξRd:Γ(ξ) = 1}(2)
Interior points: Xi={ξRd\(XeXb)}
By construction Γ(·)increases monotonically with increasing
distance from the center ξcand has a continuous first-order
partial derivative (C1smoothness). In this paper, we define:
Γ(ξ) =
ik/R(ξ))2pwith pN+(3)
with R(ξ)the distance from a reference point ξrwithin the
obstacle to the surface (Γ(ξ) = 1) in direction r(ξ)(Fig. 3a).
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(ξ).. ed1(ξ)],r(ξ) = ξξr
and the tangents e(·)(ξ)form a d1 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
Γ(ξ)=2 Γ(ξ)=3
(a) Initial System
(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 ˙
(a) No reference (b) Reference center (c) Reference top
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 ξrXimarked 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(ξ) = 11/Γ(ξ)λ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
ξrXi. 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 ξrXi
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
(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):
XsRd={ξ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. {{ξ}0Rd\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|:
cosε(ξ)max(λe(ξ)) = 2|f(ξ)|
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].
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. 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 :
wo(ξ) = 1 and wo(ξXb,ˆo) = (1o=ˆo
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
The magnitude is evaluated by the weighted mean:
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 κ(·)Rd1that projects the modulated DS
from each obstacle onto a (d1)-dimensional hyper-sphere
with radius π(Fig. 5). 3
ξo,ξ) = arccosn˙
2.. ˆ
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
d1(ξ)i. The
vectors ef
(·)(ξ)are chosen so as to form an orthonormal basis.
The weighted mean is evaluated in this κ-space (Fig. 5b):
κ(ξ) =
The direction vector of the modulated DS ˙
ξis then expressed
back in the original space:
n(ξ) = Rf(ξ)hcosk¯
With (13), the final velocity is evaluated as:
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 π.
𝝃𝟐, 𝛏
𝝃𝟐, 𝛏
𝝃𝟏, 𝛏 𝑓 𝜉
(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.
{{ξ}0Rd\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.
For moving obstacles the modulation is performed in the
obstacle reference frame, and then transformed to the inertial
frame [23]:
ξwith ˙
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: ˙
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:
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:
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
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.
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
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.
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.
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].
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.
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..d1, 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:
Γ(ξ) = 2
Γ(ξ) = 1
Γ(ξ) =3
Γ(ξ) > 0
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:
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 {ξ}0S({ξ}0), remains
in this hyper-plane for all times, i.e. {ξ}tS({ξ}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(ξ)R0the 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
to the surface:
dt 0T=d
dt ˜
E(ξ)1(ξξr) + ˜
(·) (·)(·)
0diag(1/R,1)D E1ξ
The first diagonal element of ˜
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: E1ξr=ξr<0d
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
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/(ξ1d1)) 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:
φ(ξ) = ˜
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+log(Γ01)Γ1log(Γ11)] (25)
Starting outside the obstacle Γ0>1, and having the goal on
the surface Γ1=1, the time results as: limΓ11tb(ξ).
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(ξ) =
Fig. 9: The derivative of the basis matrix E(ξ)and the partial
transformation ˆ
E(ξ)for ξ2>0.
Θ(ξ) = ˆ
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:
dt Θ Θ1+Θg(γ,ξ)
∂ γ Θ1(29)
where ˆ
E(ξ)1E(ξ) = diag(wr(ξ),we(ξ)). Hence the second
therm is diag(λr(ξ),λe(ξ)). The rows of the first therm ˙
and ˙
eare evaluated as:
dt r˙wee+we
dt e(30)
wr− k˙
we− k˙
with cosε(ξ) = r(ξ)T·e(ξ),˙
dt r(ξ), and ˙
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
det(Fsym) = ˙wr
tanε+λr ˙we
r) + wr
and its trace is negative definite:
tra(Fsym) = ˙wr
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
and analogously for the negative half-plane. These three con-
ditions put lower limits on w0
r(φ)and w0
There can be found wrand wewhich fulfill the conditions, i.e.
wr(φ) = we(φ) = exp ke0kmax
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(ξ) +
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:
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
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(ξ) + ˆ
ξ. 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:
where the metric ˆ
E1is 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.
[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.
[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.
[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,
[28] W. Lohmiller and J.-J. E. Slotine, On contraction analysis for nonlinear
systems, Automatica, vol. 34, no. 6, pp. 683696, 1998.
... This work was supported by EU ERC grant SAHR. 1 Additionally, collision avoidance needs to seamlessly integrate with reactive control techniques ( Figure 1). Control methods based on vector fields [1] and dynamical systems [2] have proven to be well-suited for addressing the challenges posed by such scenarios. Rather than precomputing a trajectory for the robot, these methods generate a (nonlinear) control field that is evaluated in real-time at the robot's position. ...
... Dynamical systems represent the evolution of a system's state without considering its history, effectively capturing the system's dynamics as a vector field. In robotics, dynamical systems have been employed for learning complex dynamics [3] and enforcing stability guarantees through techniques such as Lyapunov Stability [4] or Contraction Theory [2]. Force control in robotics often utilizes second-order dynamical sys-tems [5], while obstacle avoidance typically relies on firstorder systems that output desired velocities based on the current position [6]. ...
... To overcome challenges posed by intersecting, convex obstacles in three dimensions, DSM was extended to incorporate switching to surface following [44]. In our previous research, we successfully imitated the behavior of harmonic potential functions to ensure the convergence of DSM around concave (star-shaped) obstacles [2]. Furthermore, we expanded the scope of DSM to encompass indoor environments, facilitating reactive avoidance based on sensor data [28], [45]. ...
Full-text available
Controlling complex tasks in robotic systems, such as circular motion for cleaning or following curvy lines, can be dealt with using nonlinear vector fields. In this paper, we introduce a novel approach called rotational obstacle avoidance method (ROAM) for adapting the initial dynamics when the workspace is partially occluded by obstacles. ROAM presents a closed-form solution that effectively avoids star-shaped obstacles in spaces of arbitrary dimensions by rotating the initial dynamics towards the tangent space. The algorithm enables navigation within obstacle hulls and can be customized to actively move away from surfaces, while guaranteeing the presence of only a single saddle point on the boundary of each obstacle. We introduce a sequence of mappings to extend the approach for general nonlinear dynamics. Moreover, ROAM extends its capabilities to handle multi-obstacle environments and provides the ability to constrain dynamics within a safe tube. By utilizing weighted vector-tree summation, we successfully navigate around general concave obstacles represented as a tree-of-stars. Through experimental evaluation, ROAM demonstrates superior performance in terms of minimizing occurrences of local minima and maintaining similarity to the initial dynamics, outperforming existing approaches in multi-obstacle simulations. The proposed method is highly reactive, owing to its simplicity, and can be applied effectively in dynamic environments. This was demonstrated during the collision-free navigation of a 7 degree-of-freedom robot arm around dynamic obstacles
... Given a star world, obstacle avoidance can be achieved using a DS approach [10] with dynamics: ...
... Convergence to r g is guaranteed for a trajectory following (1) from any initial position r 0 ∈ F if F is a disjoint star world and no obstacle center point is contained by the line segment l(r 0 , r g ). For more information, see [10], [11]. ...
... 4) Convexification (line [10][11][12][13][14]: While convergence to the goal position is guaranteed following the dynamics (1) for any disjoint star world, the behaviour is not always the most intuitive in concave regions. To obtain a more direct path, any concave obstacle is made convex using the convex hull provided this does not violate the two conditions: 1) r 0 and r g remain exterior points of the obstacle, and 2) the resulting obstacle region does not intersect with any other obstacle. ...
Full-text available
This paper proposes a motion control scheme for robots operating in a dynamic environment with concave obstacles. A Model Predictive Controller (MPC) is constructed to drive the robot towards a goal position while ensuring collision avoidance without direct use of obstacle information in the optimization problem. This is achieved by guaranteeing tracking performance of an appropriately designed receding horizon path. The path is computed using a guiding vector field defined in a subspace of the free workspace where each point in the subspace satisfies a criteria for minimum distance to all obstacles. The effectiveness of the control scheme is illustrated by means of simulation.
... In this section, we aim to show the steps that we have taken in this direction, which we plan to study deeper in future work. More specifically, we tested two extensions: 1) Obstacle avoidance: multiple obstacle avoidance methods have been proposed for motions modeled as dynamical systems, and it is an active field of research [44], [45], [46], [47]. We test CONDOR with one of these extensions [44] and observe that it works properly. ...
... Obstacle avoidance for motions modeled as dynamical systems is a problem that has been addressed in the literature [44], [61], [46]. Any of these methods can be combined with our proposed framework. ...
Learning from humans allows non-experts to program robots with ease, lowering the resources required to build complex robotic solutions. Nevertheless, such data-driven approaches often lack the ability of providing guarantees regarding their learned behaviors, which is critical for avoiding failures and/or accidents. In this work, we focus on reaching/point-to-point motions, where robots must always reach their goal, independently of their initial state. This can be achieved by modeling motions as dynamical systems and ensuring that they are globally asymptotically stable. Hence, we introduce a novel Contrastive Learning loss for training Deep Neural Networks (DNN) that, when used together with an Imitation Learning loss, enforces the aforementioned stability in the learned motions. Differently from previous work, our method does not restrict the structure of its function approximator, enabling its use with arbitrary DNNs and allowing it to learn complex motions with high accuracy. We validate it using datasets and a real robot. In the former case, motions are 2 and 4 dimensional, modeled as first and second order dynamical systems. In the latter, motions are 3, 4, and 6 dimensional, of first and second order, and are used to control a 7DoF robot manipulator in its end effector space and joint space. More details regarding the real-world experiments are presented in:
... From any state that the robot finds itself in, the vector field can then steer the robot back towards the desired imitation behavior, without the need for path replanning with classical approaches. Furthermore, the learnt vector field can be modulated in real-time [26], [19], [28] in order to avoid collisions with obstacles. ...
... We point out that it is possible to use alternative modulation methods that come with different guarantees and drawbacks. In [26], [19] for instance, the authors use a multiplicative modulation function that preserves equilibrium points in the case of convex or concave obstacles. ...
... Convergence to r g is guaranteed for a trajectory following (1) from any initial position, r 0 ∈ F, if E is a DSW and no obstacle center point is contained by the line segment l[r 0 , r g ]. For more information, see [19], [20]. ...
Full-text available
This article addresses the obstacle avoidance problem for setpoint stabilization and path-following tasks in complex dynamic 2-D environments that go beyond conventional scenes with isolated convex obstacles. A combined motion planner and controller is proposed for setpoint stabilization that integrates the favorable convergence characteristics of closed-form motion planning techniques with the intuitive representation of system constraints through Model Predictive Control (MPC). The method is analytically proven to accomplish collision avoidance and convergence under soft conditions, and it is extended to path-following control. Various simulation scenarios using a non-holonomic unicycle robot are provided to showcase the efficacy of the control scheme and its improved convergence results compared to standard path-following MPC approaches with obstacle avoidance.
... T HIS paper demonstrates how to incorporate distance functions and image morphing techniques (transfinite interpolation [1]) to derive control algorithms for soft robots performing both shape formation and shape morphing tasks. Distance functions have been employed in modeling solids [2], mesh generation [3], topology optimization [4], SLAM and path planning applications [5], dynamic system planning [6], and rendering and animation [7]. In this work, the distance functions are implemented via R-functions [8]. ...
Full-text available
In this paper, we introduce a new approach for soft robot shape formation and morphing using approximate distance fields. The method uses concepts from constructive solid geometry, R-functions, to construct an approximate distance function to the boundary of a domain in $\Re^d$. The gradients of the R-functions can then be used to generate control algorithms for shape formation tasks for soft robots. By construction, R-functions are smooth and convex everywhere, possess precise differential properties, and easily extend from $\Re^2$ to $\Re^3$ if needed. Furthermore, R-function theory provides a straightforward method to creating composite distance functions for any desired shape by combining subsets of distance functions. The process is highly efficient since the shape description is an analytical expression, and in this sense, it is better than competing control algorithms such as those based on potential fields. Although the method could also apply to swarm robots, in this paper it is applied to soft robots to demonstrate shape formation and morphing in 2-D (simulation and experimentation) and 3-D (simulation).
... Algorithms based on harmonic potential flow can deal with multiple moving convex and star-shaped concave obstacles by applying a dynamic modulation matrix, dependent on the characteristics of the obstacles, to the original dynamic system ( [37,38]). However, this velocity-based controller does not take into account the inertia of the robot or more realisitc kino-dynamic constraints. ...
Full-text available
This report aims at solving task allocation and obstacle avoidance problems in a general context whose applications include disaster responses or packages deliveries by unmanned aerial / ground vehicles in dynamic unknown environments. More formally, we design a task allocation framework with collision avoidance capabilities in an environment populated by multiple mobile obstacles. First, we propose a decentralized auction-based algorithm for the solution of dynamic task allocation problems for spatially distributed multi-agent systems. In our approach, each member of the multi-agent team is assigned to at most one task from a set of spatially distributed tasks, while several agents can be allocated to the same task. The task assignment is dynamic since it is updated at discrete time stages (iterations) to account for the current states of the agents as the latter move towards the tasks assigned to them at the previous stage. Our proposed methods can find applications in problems of resource allocation by intelligent machines such as the delivery of packages by a fleet of unmanned or semi-autonomous aerial vehicles. In our approach, the task allocation accounts for both the cost incurred by the agents for the completion of their assigned tasks (e.g., energy or fuel consumption) and the rewards earned for their completion (which may reflect, for instance, the agents' satisfaction). We propose a Greedy Coalition Auction Algorithm (GCAA) in which the agents possess bid vectors representing their best evaluations of the task utilities. The agents propose bids, deduce an allocation based on their bid vectors and update them after each iteration. The solution estimate of the proposed task allocation algorithm converges after a finite number of iterations which cannot exceed the number of agents. We use numerical simulations to illustrate the effectiveness of the proposed task allocation algorithm (in terms of performance and computation time) in several scenarios involving multiple agents and tasks distributed over a spatial 2D domain. Secondly, we present an algorithm for local motion planning in environments populated by moving elliptical obstacles whose velocity, shape and size may change with time. We base the algorithm on a collision avoidance vector field (CAVF) that aims to steer an agent to a desired final state whose motion is described by a double integrator kinematic model. In addition to handling multiple obstacles, it is applicable in bounded environments for more realistic applications (e.g., motion planning inside a building). We also incorporate a method to deal with agents whose control input is limited so that they safely navigate around the obstacles. To showcase our approach, extensive simulations results are presented in 2D and 3D scenarios.
Robotic surgery is a promising direction to improve surgeons and assistants’ daily life with respect to conventional surgery. In this work, we propose solo laparoscopic surgery in which two robotic arms, controlled via haptic foot interfaces, assist the task of the hands. Such a system opens the door for simultaneous control of four laparoscopic tools by the same user. Each hand controls a manipulative tool while a foot controls an endoscope/camera and another controls an actuated gripper. In this scenario, the surgeon and robots need to work collaboratively within a concurrent workspace, while meeting the precision demands of surgery. To this end, we propose a control framework for the robotic arms that deals with all the task- and safety-related constraints. Furthermore, to ease the control through the feet, two assistance modalities are proposed: adaptive visual tracking of the laparoscopic instruments with the camera and grasping assistance for the gripper. A user study is conducted on twelve subjects to highlight the ease of use of the system and to evaluate the relevance of the proposed shared control strategies. The results confirm the feasibility of four-arm surgical-like tasks without extensive training in tasks that involve visual-tracking and manipulation goals for the feet, as well as coordination with both hands. Moreover, our study characterizes and motivates the use of robotic assistance for reducing task load, improving performance, increasing fluency, and eliciting higher coordination during four-arm laparoscopic tasks.
Conference Paper
Full-text available
We reconsider the problem of reactive navigation in sphere worlds, i.e., the construction of a vector field over a compact, convex Euclidean subset punctured by Euclidean disks, whose flow brings a Euclidean disk robot from all but a zero measure set of initial conditions to a designated point destination, with the guarantee of no collisions along the way. We use power diagrams, generalized Voronoi diagrams with additive weights, to identify the robot’s collision free convex neighborhood, and to generate the value of our proposed candidate solution vector field at any free configuration via evaluation of an associated convex optimization problem. We prove that this scheme generates a continuous flow with the specified properties. We also propose its practical extension to the nonholonomically constrained kinematics of the standard differential drive vehicle.For more information: Kod*lab
Full-text available
A navigation function is a scalar valued function on a robot configuration space which encodes the task of moving to a desired destination without hitting any obstacles. Our program of research concerns the construction of navigation functions on a family of configuration spaces whose “geometric expressiveness” is rich enough for navigation amidst real world obstacles. A sphere world is a compact connected subset of En whose boundary is the finite union of disjoint (n-1)-spheres. In previous work we have constructed navigation functions for every sphere world. In this paper we embark upon the task of extending the construction of navigation function to “star worlds.” A star world is a compact connected subset of En obtained by removing from a compact star shaped set a finite number of smaller disjoint open star shaped sets.This paper introduces a family of transformations from any star world into a suitable sphere world model, and demonstrates that these transformations are actually analytic diffeomorphisms. Since the defining properties of navigation functions are invariant under diffeomorphism, this construction, in composition with the previously developed navigation function on the corresponding model sphere world, immediately induces a navigation function on the star world. For more information: Kod*Lab
Full-text available
Given a convex potential in a space with convex obstacles, an artificial potential is used to navigate to the minimum of the natural potential while avoiding collisions. The artificial potential combines the given natural potential with potentials that repel the agent from the border of the obstacles. This is a popular approach to navigation problems because it can be implemented with spatially local information -- as opposed to most other approaches that require global knowledge of the environment. Artificial potentials can, however, have local minima that prevent navigation to the intended destination. This paper derives generic conditions that guarantee artificial potentials with a single minimum that coincides with the minimum of the natural potential. The qualitative implication of these general conditions is that artificial potentials succeed when either the condition number of the natural potential is not large and the obstacles do not have sharp corners or when the destination is not close to the border of an obstacle. Numerical analyses explore the practical value of these theoretical conclusions.
Full-text available
A path planning and tracking framework is presented in order to maintain a collision free path for autonomous vehicles. For path planning approaches, a 3D virtual dangerous potential field is constructed as a superposition of trigonometric functions of the road and the exponential function of obstacles, which can generate a desired trajectory for collision avoidance when a vehicle collision with obstacles is likely to happen. Next, in order to track the planned trajectory for collision avoidance maneuvers, the path tracking controller formulates the tracking task as a Multi-constrained Model Predictive Control (MMPC) problem, and calculated the front steering angle to prevent the vehicle from colliding with a moving obstacle vehicle. Simulink and CarSim simulations are conducted in the case where moving obstacles exist. The simulation results show that the proposed path planning approach is effective for many driving scenarios and the MMPC-based path-tracking controller provides dynamic tracking performance and maintains good maneuverability.
This work introduces a novel approach to the solution of the navigation problem by mapping an obstacle-cluttered environment to a trivial domain called the point world, where the navigation task is reduced to connecting the images of the initial and destination configurations by a straight line. Due to this effect, the underlying transformation is termed the “navigation transformation.” The properties of the navigation transformation are studied in this work as well as its capability to provide—through the proposed feedback controller designs—solutions to the motion- and path-planning problems. Notably, the proposed approach enables the construction of temporal stabilization controllers as detailed herein, which provide a time abstraction to the navigation problem. The proposed solutions are correct by construction and, given a diffeomorphism from the workspace to a sphere world, tuning free. A candidate construction for the navigation transformation on sphere worlds is proposed. The provided theoretical results are backed by analytical proofs. The efficiency, robustness, and applicability of the proposed solutions are supported by a series of experimental case studies.
Autonomous dynamical systems (DS) has emerged as an extremely flexible and powerful method for modeling robotic tasks. Task execution of DS models is typically done in an open-loop manner in combination with standard low level controller, e.g. position controller or impedance controller. Such an arrangement has two important drawbacks: 1) it is not passive and 2) the DS model cannot respond to physical perturbations on the robot body. These can imply severe consequences in motion tasks involving expected or unexpected contact with objects whose position is unknown and dynamic. We propose a novel control architecture that closes the loop around the DS, ensures passivity and allows intuitive tuning of the robot impedance. We evaluate our approach in a comparative study in an uncertain manipulation task with unexpected contact.
Dynamical Systems (DS) for robot motion modeling are a promising approach for efficient robot learning and control. Our focus in this paper is on autonomous dynamical systems, which represent a motion plan without dependency on time. We develop a method that allows to locally reshape an existing, stable nonlinear autonomous DS while preserving important stability properties of the original system. Our system is based on local transformations of the dynamics. We propose an incremental learning algorithm based on Gaussian Processes for learning to reshape dynamical systems using this representation. The approach is validated in a 2d task of learning handwriting motions, a periodic polishing motion and in a manipulation task with the 7 degrees of freedom Barrett WAM manipulator.