See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/233966434

Integrating an Anti-Collision System Based on Laser Time-Of-Flight Sensor

in an Industrial Robot Controller

Conference Paper · January 2012

CITATIONS READS

4 721

6 authors, including:

Luca Bascetta Paolo Rocco

Politecnico di Milano Politecnico di Milano
133 PUBLICATIONS 1,890 CITATIONS 226 PUBLICATIONS 4,351 CITATIONS

SEE PROFILE SEE PROFILE

Andrea Maria Zanchettin Anders Robertsson

Politecnico di Milano Lund University
132 PUBLICATIONS 3,106 CITATIONS 256 PUBLICATIONS 3,577 CITATIONS

SEE PROFILE SEE PROFILE

Some of the authors of this publication are also working on these related projects:

Adaptive Control View project

ROAMFREE View project

All content following this page was uploaded by Andrea Maria Zanchettin on 06 June 2016.

The user has requested enhancement of the downloaded file.

 Integrating an anti-collision system based on
laser Time-Of-Flight sensor in an industrial robot
controller ?
Alberto Barcellini ∗ Luca Bascetta ∗ Marco Raymo ∗ Paolo Rocco ∗
Andrea Maria Zanchettin ∗ Anders Robertsson ∗∗
∗ Dipartimento di Elettronica e Informazione, Politecnico di Milano,
Piazza L. Da Vinci 32, 20133, Milano, Italy
{bascetta, rocco, zanchettin}@elet.polimi.it
∗∗ Department of Automatic Control, Lund University,
22100, Lund, Sweden
andersro@control.lth.se

Abstract:
Safe coexistence of industrial robots and human operators in the same workspace is one of the long
standing goals of robotics research. One way to enforce safety is to endow the robotic system with
additional sensors that can to some extent monitor the environment surrounding the robot and allow
fast reaction to unexpected obstacles. This paper discusses the design of one such anti-collision system
based on a laser Time-Of-Flight distance sensor. Integration of the external controller in the hardware
and software architecture of a commercial industrial robot controller is discussed. Experimental results
show fast response of the system to various obstacles.

1. INTRODUCTION human robot interaction issues are given in De Santis et al.

(2008), Bicchi et al. (2008), Pervez and Ryu (2008).
The adoption of industrial robots in small companies can be Active physical human-robot interaction control is generally
significantly fostered if humans and robots are allowed to share performed through exteroceptive sensors mounted above or
the same workspace, without artificial protective barriers. This near the robot, or on the robot itself. Systems based on sensors
new scenario would open the path to new applications as well mounted on the robot itself, usually contact (like force/torque)
as to a radical shift of paradigm in the design of the overall and non-contact (like ultrasonic or capacitive) measurement de-
robotic cell. Situations where humans and robots work side by vices, are intrinsically exempt from occlusions. Capacitive and
side or partly cooperate to fulfill a common task might then ultrasonic devices are however highly sensitive to environment
become feasible. Physical human-robot interaction De Santis conditions (e.g. material properties), which is not the case for
et al. (2008) is in fact one of the most active research areas in laser Time Of Flight (TOF) distance sensors.
robotics, where researchers are primarily engaged with issues
related to safety of the human. TOF sensors are based on the measurement of the time needed
for an emitted signal to be received (by reflection) by the
Safety in the interaction can be, to some extent, gained in a emitting station. Physical principle might be ultrasound, radar,
passive way, i.e. with inherently safe lightweight robots, built and optical, like laser sensors. In the robotic area, an application
with mechanical solutions intended to minimize the danger of TOF sensors in mobile robotics is reported in Larsson
potential of impacts Bicchi and Tonietti (2004). Consequences et al. (1996). Use of TOF cameras (i.e. devices that combine
of impacts between robots and humans are discussed in e.g. the functionalities of a camera system with those of a TOF
Haddadin et al. (2007), Oberer and Schraft (2007), Zinn et al. matrix of distance sensors) is described in May et al. (2006),
(2002). Alternatively, safety can be gained with an active ap- with reference to mobile robotics, and in Winkler (2007) with
proach, endowing robotic manipulators with additional sensors reference to a system designed for safe interaction between
that are able to sense the position of the robot relative to obsta- humans and robotic manipulators. In Fuchs et al. (2010) the
cles (including humans), and using the resulting information in use of a TOF camera in combination with a lightweight robot
suitably designed control systems. Significant contributions in for a bin picking application is reported, while Maldonado et al.
the field include Ikuta et al. (2003), Heinzmann and Zelinsky (2010) presents a system that uses 3D perception with TOF
(2003), Kulic and Croft (2006). In Lacevic and Rocco (2010) camera and torque sensors in the fingers to reliably manipulate
the new concept of kinetostatic danger field has been proposed, a large set of unmodeled objects. In Bascetta et al. (2010) an
as a measure of danger in the vicinity of a moving robot, whose anti-collision system based on laser TOF sensor is presented
minimization can be used as a design criterion to shape an for a simple single degree of freedom system.
active control strategy. Comprehensive overviews of physical
The present paper contributes presenting the integration of an
? The research leading to these results has received funding from the European anti-collision method based on a laser TOF distance sensor in
Community’s Seventh Framework Programme FP7/2007-2013 - Challenge 2 - an industrial robot controller. Similar to Bascetta et al. (2010),
Cognitive Systems, Interaction, Robotics - under grant agreement No 230902 - a protective barrier, referred to as virtual wall, is built around
ROSETTA.
an obstacle, which is able to return a virtual force based on (1985), Ferretti et al. (2004), Ferretti et al. (2009). For this,
the sensed distance. An admittance controller Ferretti et al. consider the block diagram sketched in Fig. 2, where xr is
(2009) on the sensed virtual force, conveniently combined a reference trajectory, x is the actual robot position, d is the
with the system that reproduces the virtual wall, yields the sensed distance from an obstacle, Fw is the force delivered by
correction to the position set-points along the direction of the virtual wall, Gd is an admittance filter. Assuming that the
the sensor’s ray, i.e. the approach direction on the robot end bandwidth of the motion control system is large enough to
effector. While the system lends itself to easy implementation, neglect its dynamic effects, a simple inspection of the block
the actual integration in the architecture of an industrial robot diagram yields:
controller has been possible thanks to a recently developed x ' xr − Gd (s)Fw (3)
open version of the ABB IRC5 industrial robot controller. or:
This integration is described in this paper along with several Fw ' Gd (s)−1 (xr − x) (4)
experiments which show effectiveness and good response of the
proposed system.
Control System
2. VIRTUAL WALL
Wall
The core of the anti-collision system is the definition of a
“virtual wall” i.e. a protective area around an obstacle where Fig. 2. Block diagram of an admittance controller fed by the
the robot senses artificial (virtual) repulsive forces, which tend virtual force
to move it away from the obstacle (see Bascetta et al. (2010)).
Thus the dynamics expressed by the admittance filter Gd are
Fig. 1 shows the interaction between the robot and an obstacle
imposed between position error and virtual force. Letting:
placed along its rectilinear trajectory. In this scheme, d de-
notes the measured (for instance through the laser TOF sensor) 1
Gd (s) = (5)
distance, c the thickness of the virtual wall, µ = c − d the Md s2 + Dd s + Kd
penetration (if positive) of the robot inside the virtual wall, Fw the dynamics of a typical mass-spring-damper are assigned.
the resulting virtual force returned by the wall.
3.2 Tuning the impedance controller
Fw Wall
Notice that the distance d in Fig. 2 obviously depends on the
TOF d
Robot Robot Obstacle position itself of the robot, which implies that a further loop
is closed on the system. This is shown in Fig. 3, where xo is
µ the position of the obstacle. It is straightforward to derive the
c characteristic polynomial of this closed loop system as:
Md s2 + (Dd + Dw )s + Kd + Kw (6)
Fig. 1. Interaction between the robot and the virtual wall
≈1
The virtual wall can be assigned elastic as well as dissipative
Control System
properties, which means that Fw can be computed as:
 Wall

Kw µ + Dw µ̇ if µ > 0
Fw = (1)
0 if µ < 0
Fig. 3. Block diagram of the admittance controller with the
Let M be the effective mass of the robot in motion, dmin a explicit computation of the distance d
minimum allowed distance between the robot and the obstacle,
and Fmax the maximum force the actuator of the robot can
The damping factor of the roots of this polynomial is:
deliver. The gains Kw and Dw of the virtual wall can be selected
assuming that when the minimum distance dmin is reached, the 1 Dd + Dw
ζc = p (7)
virtual wall applies the maximum force Fmax , and that the mass- 2 (Kd + Kw )Md
spring-damper system formed by the robot and the virtual wall The desired mass Md can be set equal to the actual mass M of
has a critical damping: the robot (no mass scaling is needed). Defining the damping

factors of the dynamics of the virtual wall and of the desired
 Kw = Fmax

c − dmin dynamics of the impedance law as ζw and ζd , respectively,
(2)
 √ where:

Dw = 4MKw 1 Dw 1 Dd
ζw = √ , ζd = √ (8)
2 Kw M 2 Kd M
3. VIRTUAL IMPEDANCE CONTROLLER the following relation can be obtained:
√
ζw + αζd
3.1 Imposing the virtual impedance ζc = √ (9)
1+α
The virtual force can be used in an anti-collision system in order where α = KKwd . If we select the damping Dd so as to assign
to enforce a desired dynamic behaviour of the robotic system in critical damping to the dynamics of the admittance filter (ζd =
response to this force. This can be accomplished through the 1): p
well known tools of the admittance/impedance control Hogan Dd = 4Kd M (10)
taking into account (2), it turns out that: between the penetration c − d and the variation ∆ of position
√ set-point imposed by the scheme in Fig. 5 is correctly given by
1+ α g
ζc = √ > 1, ∀α > 0 (11) transfer function G(s) = 1+sT . The saturation limit is then set
1+α to a desired distance between the robot and the obstacle deq .
Thus the closed loop system has real eigenvalues when the Since in steady state the input to the integrator is equal to zero,
parameters of the virtual wall and of the admittance filter are the steady state condition of the system implies that the robot
selected as described above. stops at a desired distance deq from the obstacle, provided that
the following relation holds between the desired distance deq ,
3.3 Simplification the related penetration in the virtual wall c − deq , and the gain
of the controller g:
The parameter α = KKwd can be tuned in order to provide a further g=
deq
(15)
simplification of the controller. The transfer function from the c − deq
distance d to the variation imposed to the position set-point, The inclusion of a saturation calls for a supplementary stability
based on the tuning rules already introduced, is given by: analysis of the closed loop system. With reference to Fig. 6 the
1
q
Kw closed loop behaviour of the controlled manipulator has been
s +
r
Dw s + Kw Kw 2 M represented with a first order unitary gain transfer function:
G(s) = =2 2 (12)
Md s2 + Dd s + Kd M x(s)
 q
Kw
ω
s+ α M = (16)
xr (s) − ∆ s + ω̄
The above transfer function can be approximated with the This results into the block diagram in Fig. 7, where transfer
transfer function: function F(s) takes the expression:
s + ω̄
r
Kw 1 F(s) = 2 (17)
Gapp (s) = q (13) T s + T ω̄s + gω̄
αM s + α Kw
M Since F(s) is strictly positive real for every positive value of the
which has the same gain as G. Notice that selecting α = parameters, as it can be easily verified, the system is absolutely
0.25 (i.e. the virtual wall 4 times stiffer than the prescribed stable based on the circle criterion.
impedance stiffness), there is no approximation in simplifying
G as in Gapp .
The controller can thus be elaborated as in Fig. 4, where:
g 1

xr x
Rpos, Rvel
Sat
x0 d

Fig. 6. Overall control scheme with saturation

Fig. 4. Simplified block diagram

F(s)
g
G(s) = (14)
1 + sT
and parameters g and T have to be determined. Sat
3.4 Saturating the output of the impedance controller Fig. 7. Control loop for absolute stability analysis
The actual implementation of the virtual impedance controller
requires a modification of the control scheme, whereby transfer 3.5 Application to a 6 d.o.f. manipulator
function G(s) is replaced by the block diagram shown in Fig. 5.
The saturation in the feedback loop is introduced in order to While the above discussion on the design of the virtual
c-d >0
impedance controller fits the 1 dof scenario, a simple adap-
tation allows its application to the scenario considered in this
1 1 ∆ work, namely where the distance sensor is mounted on the end-
c-d g s effector of the robot along the approach direction. The scalar
position variation ∆ computed by the virtual impedance con-
troller is projected onto the approach vector, so as to determine
∆
Sat separate variations in the three Cartesian position coordinates.
No modifications of the end effector orientation is applied by
Fig. 5. Block diagram of the virtual impedance controller with the impedance controller.
saturation
4. EXPERIMENTAL SETUP
avoid that the steady state distance of the manipulator from
a fixed obstacle depend on the position reference. Notice that Experiments have been made to test the anti-collision system
when the saturation is inactive (linear regime) the relation described in this work in real situations on an industrial robot.
The robot is an ABB IRB 140 robotic arm, with a nominal the switch T1 is closed in Fig. 10), or to read and modify these
payload of 6 kg, endowed with an ABB IRC5 controller. A data (both switches are closed).
commercial laser TOF sensor has been mounted on the robot The software architecture of the anti-collision system devel-
end-effector (see Fig. 8): the sensor (BOD 63M-LA02-S115)
is manufactured by Balluff. It has a working range from 200
mm to 2 m, nominal response time less than 2 ms, nominal
resolution less than 1 mm. The sensor output is read through a
National Instruments PCI-6071E acquisition board: the overall
hardware architecture is sketched in Fig. 9.

./images/extctrl.png

Fig. 8. The laser sensor mounted on the robot end effector

Fig. 10. External controller interface

oped on the external controller is sketched in Fig. 11. A trans-

formation matrix from joint to motor positions denoted with
K, and its inverse K −1 , are included in the architecture. The
impedance controller is fed by the sensor measurement. The
output of the impedance controller is then projected on the ap-
proach vector, computed through a direct kinematics algorithm.
The resulting variations of the Cartesian position of the end
effector are added to the desired values which are determined in
a Trajectory Generation module, together with the desired ori-
entation. Inverse kinematics is then needed in order to generate
equivalent commands at joint level, to be imposed to the robot
controller.

Fig. 9. The overall hardware architecture

The external PC runs under Linux operating system with the

Xenomai patch, to obtain a hard real time system. It is in-
terfaced to the acquisition board through appropriate Analogy ./images/sw-architecture.png
drivers and to the IRC5 controller through an Ethernet based
interface Blomdell et al. (2010) partly developed within the
ROSETTA project by Lund University (see Fig. 10). Thanks
to this interface, the user can develop his/her controller using
a Simulink GUI on the external controller, and then compile it
to get the executable code. This code runs in real time dialogue
with the IRC5 controller at a 250 Hz frequency. As the interface
between the ABB controller and the PC is bidirectional, the two
systems exchange several data at the joint level (motor and joint
actual and commanded positions, velocities and torques). The
interface allows a few interaction modes between the external
unit and the ABB controller, where the external unit can be Fig. 11. Software architecture of the anti-collision system
allowed just to read the robot data but not to modify them (only
 5. EXPERIMENTAL RESULTS

5.1 Fixed obstacle

A first experiment has been performed using a fixed obstacle

(see Fig. 12). A virtual wall with a thickness c = 0.25 m and a
minimum allowed distance dmin = 0.05 m has been adopted,
while the equilibrium distance deq has been set to 0.167 m.
Notice that based on (15) the value of g is equal to 2. A ./images/fixed_obstacle_distance.png
straight line has been commanded with a trapezoidal velocity
profile, which imposes to the end effector a maximum velocity
of 0.75 m/s and an initial acceleration of 4 m/s2 . Fig. 13

Fig. 14. Fixed-obstacle experiment: distance from the obstacle

shows the originally commanded trajectory and the modified

one, along the approach direction, for this experiment. The
Fig. 12. Robot postures during the fixed-obstacle experiment position of the moving obstacle is reported too, as computed
from the measured distance. The measured distance is reported
shows, along the approach direction, the reference trajectory in Fig. 17, instead. It is interesting to note that despite the sensor
and the trajectory modified by the impedance controller when output is somewhat noisy, the modified trajectory is smoother,
the virtual wall is entered. Notice that the anti-collision system thanks to the low-pass filtering action of the virtual impedance
commands a modification of the trajectory which keeps the end- controller.
effector at a safe distance from the obstacle (larger than the
minimum allowed one). Fig. 14 shows the distance from the

./images/fixed_obstacle_pos.png Fig. 15. Robot postures during the moving-obstacle experiment

Fig. 13. Fixed-obstacle experiment. Red dashed line: original

trajectory; black solid line: obstacle; black dash-dot line: ./images/moving_obstacle_pos.png
virtual wall and minimum allowed distance from obstacle;
blue solid line: modified trajectory
obstacle as measured by the TOF sensor. Despite the high-speed
trajectory, it is clear enough that the anti-collision system acts
promptly and keeps the distance of the end-effector from the
obstacle above the minimum prescribed value, making it reach
the prescribed steady state value.

5.2 Moving obstacle Fig. 16. Moving-obstacle experiment. Red dashed line: original
trajectory; black solid line: obstacle; black dash-dot line:
In this experiment the end effector was commanded a cyclic minimum allowed distance from obstacle; blue solid line:
motion on the vertical axis, but an obstacle with random motion modified trajectory
was perceived along the approach vector (see Fig. 15). Fig. 16
 ./images/moving_obstacle_distance.png ./images/disappearing_obstacle_pos.png

Fig. 17. Moving-obstacle experiment: distance from the obsta- Fig. 19. Disappearing-obstacle experiment. Red dashed line:
cle original trajectory; black solid line: obstacle; black dash-
5.3 Disappearing obstacle dot line: minimum allowed distance from obstacle; blue
solid line: modified trajectory
This experiment tests the robustness of the anti-collision system
when an obstacle suddenly disappears (see Fig. 18). Fig. 19
Bicchi, A. and Tonietti, G. (2004). Fast and soft arm tactics -
shows the originally commanded trajectory and the modified
dealing with the safety-performance trade-off in robot arms
one, along the approach direction, for this experiment. Once
design and control. IEEE Robotics and Automation Mag-
the TOF sensor does not reveal the obstacle presence, the
azine, Special issue on Dependability in Human-Friendly
modified position reference approaches the original one. This
Robots, 11, 22–33.
approaching phase is governed by a dynamics which is entirely
Blomdell, A., Dressler, I., Nilsson, K., Robertsson, A., and
associated to the time constant T of the filter (T = 0.6 in these
Dressler, I. (2010). Flexible application development and
experiments).
high-performance motion control based on external sensing
and reconfiguration of abb industrial robot controllers. In
Proc. ICRA 2010 Workshop on Innovative Robot Control
Architectures for Demanding (Research) Applications. An-
chorage, AK.
De Santis, A., Siciliano, B., De Luca, A., and Bicchi, A. (2008).
An atlas of physical human-robot interaction. Mechanism
and Machine Theory, 43, 253–270.
Ferretti, G., Magnani, G., and Rocco, P. (2004). Impedance
control for elastic joints industrial manipulators. IEEE Trans.
Fig. 18. Robot postures during the disappearing-obstacle exper- on Rob., 20, 488–498.
iment Ferretti, G., Magnani, G., and Rocco, P. (2009). Assigning
virtual tool dynamics to an industrial robot through an ad-
6. CONCLUSIONS mittance controller. In Proc. IEEE Int. Conf. Adv. Robot.
Fuchs, S., Haddadin, S., Keller, M., Parusel, S., Kolb, A., and
Suppa, M. (2010). Cooperative bin-picking with time-of-
An anti-collision system has been presented in this paper. While
flight camera and impedance controlled dlr lightweight robot
the algorithm has been conceived for easy implementation,
iii. In Proc. RSJ/IEEE Int. Conf. on Intell. Rob. Syst., 4862–
actual real-time integration of such sensor based controllers in
4867.
an industrial robot control is difficult in general. The paper has
Haddadin, S., Albu-Schäffer, A., and Hirzinger, G. (2007).
reported on an application where a laser TOF distance sensor
Safety evaluation of physical human-robot interaction via
has been used to let an industrial robot react in a controlled
crash-testing. In Proc. Robotics Science and Syst. Conf.
way to unexpected obstacles. Experiments have shown good
Heinzmann, J. and Zelinsky, A. (2003). Quantitative safety
performance of the method.
guarantees for physical human-robot interaction. Int. Journ.
Robotics Research, 22, 479–504.
REFERENCES
Hogan, N. (1985). Impedance control: an approach to manip-
Bascetta, L., Magnani, G., Rocco, P., Migliorini, R., and Pela- ulation. part i: Theory. ASME J. of Dyn. Syst., Meas., and
gatti, M. (2010). Anti-collision systems for robotic ap- Contr., 107, 1–24.
plications based on laser time-of-flight sensors. In Proc. Ikuta, K., Nokata, M., and Ishii, H. (2003). Safety evaluation
IEEE/ASME Int. Conf. Adv. Intell. Mechatr., 278–284. method of design and control for human-care robots. Int.
Bicchi, A., Peshkin, M., and Colgate, E. (2008). Safety for Journ. Robotics Research, 22, 281–297.
physical human-robot interaction. In Handbook of Robotics. Kulic, D. and Croft, E. (2006). Real-time safety for human-
Springer. robot interaction. Robotic Autonomous Systems, 54, 1–12.
Lacevic, B. and Rocco, P. (2010). Kinetostatic danger field -
a novel safety assessment for human-robot interaction. In
Proc. RSJ/IEEE Int. Conf. on Intell. Rob. Syst., 2169–2174.
Larsson, U., Forsberg, J., and Wernersson, A. (1996). Mobile
robot localization: integrating measurements from a time-of-
flight laser. IEEE Transactions on Industrial Electronics, 43,
422–431.
Maldonado, A., Klank, U., and Beetz, M. (2010). Robotic
grasping of unmodeled objects using time-of-flight range
data and finger torque information. In Proc. RSJ/IEEE Int.
Conf. on Intell. Rob. Syst., 2586–2591.
May, S., Werner, B., Surmann, H., and Pervölz, K. (2006).
3D time-of-flight cameras for mobile robotics. In Proc.
IEEE/RSJ Int. Conf. Intell. Robot Systems, 790–795.
Oberer, S. and Schraft, R. (2007). Robot-dummy crash tests for
robot safety assessment. In Proc. IEEE Rob. Autom. Conf.,
2934–2939.
Pervez, A. and Ryu, J. (2008). Safe physical human robot
interaction - past, present and future. Journal of Mechanical
Science and Technology, 22, 469–483.
Winkler, B. (2007). Safe space sharing human-robot coopera-
tion using a 3D time-of-flight camera. In Proc. Int. Robots
Vision Show, ISR 2007.
Zinn, M., Khatib, O., Roth, B., and Salisbury, J. (2002). Playing
it safe - human-friendly robots. IEEE Robotics Automation
Magazine, 11, 12–21.

View publication stats