Professional Documents
Culture Documents
2, APRIL 2017
AbstractThe problem of real-time motion generation to a tar- robot and the environment. This assumption is relaxed in reac-
get with obstacle avoidance is considered. A second-order dynam- tive obstacle avoidance which introduces obstacles as they are
ical system having the target as a unique globally asymptotically perceived by sensors during the robot motion and subsequently
stable equilibrium is modified in the presence of obstacles by an ad-
ditive signal whose design is based on the prescribed performance adapt to avoid collisions [6]. In the dynamical system-based
control methodology. It is proved that obstacle avoidance and dy- approach of [5], obstacle avoidance is addressed by locally
namical system stability are guaranteed and that the target remains modifying the original first order dynamics via a modulation
asymptotically stable. Simulations are utilized to reveal the cases matrix which deflects the velocity field along the tangent plane
where the proposed scheme outperforms the modulated dynami- away from the obstacle without affecting system stability. Sad-
cal system and the constrained optimization priority framework.
Experimental results further validate the theoretical findings. dle points or local minima may occur on the obstacles boundary
and at points multiple obstacles intersect the methods fails to
Index TermsCollision avoidance, motion control of manipula- provide a solution [5]. In [2] an additive term is applied to a
tors.
dynamic movement primitive (DMP) to stir the motion away
from the obstacle. The method is however restricted to point
I. INTRODUCTION mass obstacles.
In a multiple task priority framework generalized for inequal-
S ROBOTS are progressively coming out of the structured
A industrial setting and into less predictable environments
operating close to humans, it is imperative to control them ef-
ity tasks [7], obstacle avoidance is expressed as one sided in-
equality constraints on the robots velocities. In this framework
a constrained minimization of a quadratic function under equal-
ficiently so that they react in real time to unexpected changes.
ity and inequality constraints is formulated and solved by a
To this direction, dynamical systems have offered an alterna-
general purpose optimization algorithm (HQP). The solution
tive to precomputed trajectories as they generate a trajectory
does not avoid local minima. Moreover, the computational cost
during motion from a differential equation. Instead of a single
of such generalized solution is high despite improvements of
trajectory, a dynamical system offers a flow field which can
the solver reported in [8]. The elementary form of constraints
guarantee convergence to the target as well as automatically
like the joint limits and obstacles calls for their explicit con-
adapt to perturbations that are captured by external sensors, for
sideration which could lead to computationally more efficient
e.g. a change in the desired target pose [1], [2]. Dynamical sys-
algorithms, for example and in the case of joint limits, the works
tems may be further used to encode rich motion patterns learned
of [9], [10].
by demonstrations [3], [4].
In this work, an additive term is utilized for the purpose
The robustness of the dynamical systems has also been ex-
of obstacle avoidance with a second order dynamical system
ploited against perturbations for reactive obstacle avoidance [5],
having the target as a unique stable equilibrium as in [2]. In
[2]. Obstacle avoidance has been investigated since the 80s
contrast to [2] however, an obstacle is assumed to be contained
within the motion planning techniques which compute a colli-
in a convex region defined by a generalized ellipsoid. More-
sion free trajectory to the target, assuming a perfect model of the
over, the obstacle avoidance signal is designed following the
prescribed performance control methodology (PPC) introduced
Manuscript received September 10, 2016; accepted December 26, 2016. Date
of publication January 10, 2017; date of current version February 2, 2017. This in [11]. PPC has been applied in the design of robot position
paper was recommended for publication by Associate Editor J.-M. Lien and controllers [12], [13] and as it allows the designer to impose
Editor N. Amato upon evaluation of the reviewers comments. This work was bounds on the output signals of nonlinear systems has already
supported by the European Communitys Framework Programme Horizon 2020
under Grant 644938 SARAFun. been successfully utilized to impose spatial and joint limit con-
S. Stavridis is with the Department of Electrical and Computer Engineer- straints, [14], [10]. In this work, it is proved using Lyapunovs
ing, Aristotle University of Thessaloniki, Thessaloniki 54641, Greece (e-mail: theory that obstacle avoidance and modified system stability is
sotistav@ece.auth.gr).
D. Papageorgiou and Z. Doulgeri are with the Department of Electrical guaranteed and that the target remains asymptotically conver-
and Computer Engineering, Aristotle University of Thessaloniki, Thessaloniki gent. It is illustrated in simulation for non-convex scene cases
54641, Greece, and also with the Center for Research and Technology Hellas, in which it outperforms the method of dynamical system modu-
Thermi 570 01 Greece (e-mail: dimpapag@iti.gr; doulgeri@iti.gr).
Color versions of one or more of the figures in this letter are available online lation (MDS) of [5] and HQP [8]. Experimental results utilizing
at http://ieeexplore.ieee.org. a KUKA LWR4+ are also provided to illustrate the methods
Digital Object Identifier 10.1109/LRA.2017.2651172 performance.
2377-3766 2017 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See http://www.ieee.org/publications standards/publications/rights/index.html for more information.
STAVRIDIS et al.: DYNAMICAL SYSTEM BASED ROBOTIC MOTION GENERATION WITH OBSTACLE AVOIDANCE 713
II. DYNAMICAL SYSTEM MODIFICATION Fig. 2. Obstacle avoidance parameter selection for two different initial
FOR OBSTACLE AVOIDANCE positions p 0 1 (blue) and p 0 2 (red).
It is straightforward to ascertain that (12) is equivalent to d(p) > We are now ready to propose the following signal for obstacle
d(p) > d(p) t 0. Notice that by construction (0) satisfies avoidance:
(12). By defining the open set, T
uh = kh (t) (23)
= {(t) : (dT d(pT )) < (t) < d(pT ) dT }
(13) with kh > 0 a control parameter. Notice that kh can be used
we can guarantee obstacle avoidance if (t) t 0. To to regulate the flatness of the transformed error away from the
this aim, a transformed variable is utilized: boundaries and thus tune the strength of attraction to dm (p)
in a proportional way [see Fig. 1(a)]. So by tuning this gain it
(t) = T ((t)) (14) is possible to avoid the formation of local minima and induce
where the transformation function T () is a smooth, strictly motions that may not temporarily advance towards the target, as
increasing function defining a bijective mapping: shown in the cases presented in the Section III. Further notice
that determines the acting direction of the obstacle avoidance
T : (, ) (15) signal and is in
general the resultant of two vectors (21). In
T (0) = 0. (16) fact, its norm, 1 + f 2 (p) tan2 1, amplifies the obstacle
avoidance signal as we come closer to the obstacle surface to
A suitable transformation based on the natural logarithm is a level dependent on the vicinity of the current position to the
illustrated in Fig. 1(a) and given by: main ellipsoidal axes (governing the value of cos ); its direc-
tion forms angle tan1 (f (p) tan ) with p ph , hence on the
1+ surface (f (p) = 1) is in the direction of nh coming closer to
dT d(pT )
T ((t)) = ln . (17) p ph as we move away.
1 We are now ready to state the main stability result:
d(pT ) dT
Theorem 1: Consider the obstacle ph , a, b, c. The obstacle
According to the PPC methodology it is sufficient to ensure avoidance signal (23) applied to the dynamical system (1), (2)
the boundedness of (t) in order to guarantee the evolution of guarantees i) (t) t 0 and hence obstacle avoidance
(t) t 0. ii) asymptotic convergence to a stationary state iii) asymptotic
Taking into account (11), (10), the time derivative convergence to an invariant set containing the target iv) the
of (17) yields: boundedness of all closed loop signals.
T Proof. Consider the generalized state [(p pT )T p T ]T of
= (d d(p)). (18) the closed loop system p = p (p pT ) kh (t) T ,
(20) and the candidate Lyapunov function:
The time derivative of (5) and (7) respectively yields d(p) =
(pp h ) T 2m +1 T 1
pp h
p and f (p) = f (p) nh (p)
p, where: V = U + pT p (24)
2
1 where
nTh (p) = h(p) (19)
2m 1 1
U= (p pT )T (p pT ) + kh 2 (25)
is the normal vector of the level set h(p) = c, for some constant 2 2
c , at the current position. Substituting the above into (18), with the latter term representing the potential field induced by
taking into account (7) and that (p ph )T nh (p) = h(p) yields: (23) which can be viewed as a barrier Lyapunov function [see
T T Fig. 1(b)]. It is easy to establish that the derivative of (25) yields:
= p (20)
V = pT p 0, t 0. (26)
where:
The latter implies a bounded transformed variable, i.e. (t)
p ph nh (p) L and hence (t) t 0 [11]; moreover, limt p =
= w1 (p) + w2 (p) (21)
p ph nh (p) 0 leading to a stationary state. Invoking Lasalles invariance
with principle, it is clear that the system solution will converge to the
following stationary invariant set:
w1 (p) = 1 f (p) T
(p pT ) + kh (t) = 0. (27)
f (p)
w2 (p) = (22)
cos This set is consisted of the desired target pT which clearly sat-
(pp h ) n h (p)
T isfies (27) since at p = pT , (t) = 0 = (t) from (11), (10), as
where cos = pp h n h (p)
. well as isolated positions {p : p = pT (27)} which are unde-
Notice that owing to the definition of nh (p) (19) and h(p) sired local equilibria.
(4), 0 < cos 1 p, and since f (p) > 0, w2 (p) > 0. More- Remark 1: Notice that undesired local equilibria cannot con-
over, f (p) < 1 outside the obstacle, hence 1 > w1 (p) 0 hav- tain positions belonging to the scalar attractor dm (p), i.e. po-
ing the value of zero on the obstacles surface. For spheres sitions satisfying d(p) = dm (p) (10), since then (t) = 0. A
= nn hh (p)
(p) = pp h .
pp h
necessary condition for satisfying (27) away from the target,
STAVRIDIS et al.: DYNAMICAL SYSTEM BASED ROBOTIC MOTION GENERATION WITH OBSTACLE AVOIDANCE 715
Ti
uh = khi i (t)i . (28)
i=1
i
The stability proof follows the same line involving now addi-
tional potential terms for each i in the Lyapunov function. The
second term of the equilibrium set (27) is for multiple obstacles
the sum of the obstacle avoidance signals in (28). Notice that
when an obstacle is dynamically introduced, a new term is added Fig. 3. Potential fields induced by the proposed signal. (a) Potential fields
in (28). On the other hand when the desired target is changed induced by each of 3 obstacles ( = 0.3). (b) Sum of potential fields ( = 0.3).
during the motion of the robot, (28) should be recomputed for (c) Sum of potential fields ( = 0.015).
all obstacles.
The potential fields induced by each term of (28) are defined
inside the barriers ( i , i + i ) where i di (pT ) dTi [see
Fig. 1(b)] that are affected by the choice of i via i (9).
Fig. 3(a) illustrates level sets of potentials for 3 obstacles in
2D from high levels close to the obstacle surface to the lowest
level for each scalar attractor containing pT , getting high again
beyond the attractor and towards the upper barrier. These po-
tential fields define a constrained space depicted in Fig. 3(b).
Notice that the proposed methodology ensures by construc-
tion that the initial position belongs to all these potentials [see
Fig. 3(b)]. Due to the dynamic nature of the problem it is pos-
Fig. 4. Example of obstacle avoidance in the C-space. (a) Task space.
sible for a local minimum to occur if the constrained space is (b) C-space.
not connected, as it is in this example the case of = 0.015
[see Fig. 3(c)]. However, parameter i can be utilized to avoid
such cases [see Fig. 3(b)]. In practice, when a local mini- B. Extension to the Configuration Space
mum is detected (i.e. the velocity becomes zero and the po- The proposed method can be extended to the n-dimensional
sition is not at the target), increasing of khi and i leads to the configuration space (p n ) given that obstacles are mapped
solutions escape. to C-space obstacle regions that should subsequently be fitted
Remark 3: As the proposed signal for obstacle avoidance within n-dimensional ellipsoids. As an example consider a 2
affects the dynamical system by its additional attraction it is dof planar robotic arm [see Fig. 4(a)] moving from the initial
desirable to reduce its effect for obstacles that are left behind or configuration q0 to the target qT in the presence of two obsta-
are at a far distance. Algorithmic mechanisms can be used again cles mapped to two C-space obstacle regions [see Fig. 4(b)].
for this purpose. These can either take the form of direct deacti- Fig. 4(a) and (b) include the path generated by the robots mo-
vation (refer to the simulation example) or smooth elimination tion with and without the proposed obstacle avoidance signal,
e.g. by utilizing the distance from the lower bound d(p) d(p) shown in solid red line and dashed gray respectively. Notice that
in a multiplicative exponential term. the obstacle ellipsoids are avoided in the configuration space,
716 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 2, NO. 2, APRIL 2017
Fig. 5. Robots motion (2 obstacles). Black ellipse: obstacle #1, Green ellipse: obstacle #2, red dot: Initial position, blue dot: target position. (a) t = 0.
(b) t = 2 s. (c) t = 4 s. (d) t = 7 s. (e) End of motion.
hence no collision will occur with any of the links of the robot
and the obstacles.
Fig. 8. End-effector path: Proposed method (blue line), HQP (red line) MDS
(green line). (a) Side view. (b) Top view.
Fig. 10. Scene captured by vision. Green dots indicate the center of the
obstacles.
REFERENCES
[1] M. Hersch and A. Billard, Reaching with multi-referential dynam-
ical systems, Auton. Robots, vol. 25, no. 12, pp. 7183, 2008,
doi: 10.1007/s10514-007-9070-7.
[2] H. Hoffmann, P. Pastor, D. H. Park, and S. Schaal, Biologically-inspired
dynamical systems for movement generation: Automatic real-time goal
Without (red line) and with obstacle presence
Fig. 12. End-effector velocity p. adaptation and obstacle avoidance, in Proc. IEEE Int. Conf. Robot.
(blue line). Autom., 2009, pp. 25872592, doi: 10.1109/ROBOT.2009.5152423.
[3] S. M. Khansari-zadeh and A. Billard, Learning stable nonlinear dynam-
ical systems with gaussian mixture models, IEEE Trans. Robot., vol. 27,
no. 5, pp. 943957, Oct. 2011.
[4] A. J. Ijspeert, J. Nakanishi, and S. Schaal, Learning attractor landscapes
for learning motor primitives, in Proc. Adv. Neural Inf. Process. Syst.,
2002, vol. 15, pp. 15471554.
[5] S. M. Khansari-Zadeh and A. Billard, A dynamical system approach to
realtime obstacle avoidance, Auton. Robots, vol. 32, no. 4, pp. 433454,
2012, doi: 10.1007/s10514-012-9287-y.
[6] B. Siciliano and O. Khatib, Springer Handbook of Robotics. New York,
NY, USA: Springer-Verlag, 2007.
[7] O. Kanoun, F. Lamiraux, and P.-B. Wieber, Kinematic control of redun-
dant manipulators: Generalizing the task-priority framework to inequal-
ity task, IEEE Trans. Robot., vol. 27, no. 4, pp. 785792, Aug. 2011,
doi: 10.1109/TRO.2011.2142450.
[8] A. Escande, N. Mansard, and P.-B. Wieber, Hierarchical quadratic
programming: Fast online humanoid-robot motion generation, Int.
Fig. 13. Obstacle avoidance signal u h . J. Robot. Res., vol. 33, no. 7, pp. 10061028, 2014, doi:
10.1177/0278364914521306.
[9] F. Flacco, A. D. Luca, and O. Khatib, Motion control of redundant robots
the motion of the end-effector in 3D space. Moreover, similarly under joint constraints: Saturation in the null space, in Proc. IEEE Int.
to the simulations the numerical integration of the dynamical Conf. Robot. Autom., 2012, pp. 285292.
system is done on-line, initialized with the measured state of the [10] A. Atawnih, D. Papageorgiou, and Z. Doulgeri, Kinematic control of
redundant robots with guaranteed joint limit avoidance, Robot. Auton.
robot at t = 0 s; the closed loop inverse kinematics solution is Syst., vol. 79, pp. 122131, 2016, doi: 10.1016/j.robot.2016.01.006.
utilized to map the end-effector state to joint state. The path of [11] C. Bechlioulis and G. Rovithakis, Robust adaptive control of feed-
the end-effector is shown in Fig. 11(a) and (b) for both cases of back linearizable MIMO nonlinear systems with prescribed performance,
IEEE Trans. Automat. Control, vol. 53, no. 9, pp. 20902099, Oct. 2008.
motion generation with and without the obstacles presence. [12] Y. Karayiannidis and Z. Doulgeri, Model-free robot joint position regula-
Translational velocity of the end-effector is shown in Fig. 12 tion and tracking with prescribed performance guarantees, Robot. Auton.
while the obstacle avoidance signal is shown in Fig. 13. Notice Syst., vol. 60, pp. 214226, 2012.
[13] Y. Karayiannidis, D. Papageorgiou, and Z. Doulgeri, A model-free con-
that translational velocities are calculated by simple numerical troller for guaranteed prescribed performance tracking of both robot
differentiation of the position provided by KUKA software. No- joint positions and velocities, IEEE Robot. Autom. Lett., vol. 1, no. 1,
tice the effectiveness and the smoothness of the motion achieved pp. 267273, Jan. 2016, doi: 10.1109/LRA.2016.2516245.
[14] A. Theodorakopoulos, G. A. Rovithakis, and Z. Doulgeri, An impedance
by the proposed method. control modification guaranteeing compliance strictly within preselected
spatial limits, in Proc. 2015 IEEE/RSJ Int. Conf. Intell. Robots Syst.,
V. CONCLUSION 2015, pp. 22102215, doi: 10.1109/IROS.2015.7353673.
[15] D. Bullock and S. Grossberg, Neural dynamics of planned arm
In this paper we propose an obstacle avoidance signal de- movements: Emergent invariants and speed-accuracy properties during
trajectory formation, Psychol. Rev., vol. 95, no. 1, pp. 4990, 1988.
signed according to the prescribed performance control method- [16] V. Bargsten, P. Zometa, and R. Findeisen, Modeling, parameter iden-
ology, which affects the motion generated by a second order tification and model-based control of a lightweight robotic manipula-
dynamical system having the target as a unique stable equilib- tor, in Proc. 2013 IEEE Int. Conf. Control Appl., 2013, pp. 134139,
doi: 10.1109/CCA.2013.6662756.
rium. The proposed scheme, utilizing generalized ellipsoids for
the obstacles, is proved to guarantee collision avoidance and