Sensorless Robot Collision Detection
and Hybrid Force/Motion Control
∗
Alessandro De Luca Raffaella Mattone
Dipartimento di Informatica e Sistemistica
Universit
`
a di Roma “La Sapienza’
Via Eudossiana 18, 00184 Roma, Italy
{deluca,mattone}@dis.uniroma1.it
Abstract— We consider the problem of real-time detection
of collisions between a robot manipulator and obstacles of
unknown geometry and location in the environment without
the use of extra sensors. The idea is to handle a collision at
a generic point along the robot as a fault of its actuating
system. A previously developed dynamic FDI (fault detection
and isolation) technique is used, which does not require
acceleration or force measurements. The actual robot link
that has collided can also be identified. Once contact has been
detected, it is possible to switch to a suitably defined hybrid
force/motion controller that enables to keep the contact, while
sliding on the obstacle, and to regulate the interaction force.
Simulation results are shown for a two-link planar robot.
Index Terms— Collision detection, fault detection and iso-
lation, hybrid force/motion control.
I. INTRODUCTION
When a robot manipulator operates in an unstructured
environment or shares its workspace with a human user,
safety issues are of primary concern [1]. Main injuries
may occur from an accidential collision between the robot
structure and the environment (viz. humans), due to the
uncertain location of obstacles and/or unpredicted relative
motion. Avoiding such collisions requires (at least local)
knowledge of the environment geometry and the use of
computationally intensive motion planning techniques, see
e.g. [2]. Anticipating incipient collisions or detecting them
in real-time is typically based on the use of additional
external sensors, such as sensitive skins [3], on-board
vision [4], strain gauges [5], force load cells, and so on.
When an impact occurs, the resulting contact forces may
be alleviated, by pursuing a lightweight robot design [6],
possibly with distributed compliant characteristics in the
driving system and/or through a soft covering of the
links [7]. Once a collision is detected, the controller should
switch strategy and either stop robot motion or perform a
more sophisticated interaction task.
It is obviously more cost effective to be able to detect
contacts without the need of additional sensors. Comparing
the applied torque (or, the current in an electrical drive)
with the nominal model-based command (i.e., the torque
expected in the absence of collision) and looking for fast
transients so as to detect possible collision is a rather
intuitive scheme, see, e.g., [8], [9], [10], which has been
refined lately by including an adaptive control scheme [11],
∗
Work supported by the EU project EU-IST-2001-32122 IFATIS.
[12]. However, tuning of collision detection thresholds in
such schemes is difficult because of the highly varying
dynamic characteristics of the commanded torques. In
addition, a common drawback (even when robot dynamics
is perfectly known) is that the on-line torque computation
based on inverse dynamics requires acceleration measure-
ments, which introduces in practice noise (due to numer-
ical differentiation of velocity or position data) and/or an
intrinsic one-step delay in a digital implementation.
In this framework, we consider the following specific
problem.
1) Detect a collision of the robot with an unknown
environment, using only the standard proprioceptive
sensors (joint encoders). Collision may occur at any
point along the robot arm.
2) Switch and execute an interaction control task that
requires keeping the contact during motion while
regulating force at the contact point. This should be
performed without any force sensor.
The main idea that we pursue is to handle the collision
as a faulty behavior of the robot actuating system. In
fact, the dynamic effect of a cartesian contact force is
that of an additional joint torque with respect to the
commanded one. Therefore, robot actuator fault detection
and isolation (FDI) techniques [13], [14], [15] can be
used. These do not require acceleration measurements nor
inversion of the robot inertia matrix. In particular, the
FDI method based on generalized momenta [13] works
independently of the generation scheme for the nominal
torque, which may thus be any open-loop command or
feedback law. This is particularly convenient when it is
necessary to switch control strategy. The FDI scheme
produces a residual vector which is a filtered version of the
joint torques resulting from cartesian contact forces. Due to
an intrinsically decoupled structure, the components of this
residual vector contain as much information as possible on
the location of the collision force. Furthermore, during the
interaction task, the residual vector is used to decompose
the joint velocity space into complementary directions, so
that a hybrid force/motion controller can be designed. In
order to illustrate the performance of the whole approach,
simulation results are presented for a two-link planar robot
colliding and then sliding along a compliant obstacle.
Proceedings of the 2005 IEEE
International Conference on Robotics and Automation
Barcelona, Spain, April 2005
0-7803-8914-X/05/$20.00 ©2005 IEEE.
999
II. SENSORLESS COLLISION DETECTION
Consider a rigid robot manipulator having n (rotational)
joints, with associated generalized coordinates q, that may
undergo a possible contact with the environment at a
generic point of the structure. Using a Lagrangian ap-
proach, the robot dynamic model is
M(q)¨q + c(q, ˙q)+g(q)=τ + τ
c
, (1)
where M(q) > 0 is the (symmetric) inertia matrix, c(q, ˙q)
is the Coriolis and centrifugal vector, g(q) is the gravity
vector, τ is the commanded joint torque, and
τ
c
= J
T
c
(q)F
c
, (2)
is the joint torque associated to a generalized contact force
F
c
. The Jacobian matrix J
c
(q) relates the linear and angular
velocity of a frame Σ
c
located at the contact point P
c
to
the joint velocity ˙q. Both terms in the right-hand-side of
eq. (2) are supposed to be unknown.
The generalized momentum p = M (q)˙q associated to
the mechanical system (1) satisfies the first-order equation
˙p = τ + τ
c
− α(q, ˙q), (3)
where the components of α are given by [13]
α
i
= g
i
(q) −
1
2
˙q
T
∂M (q)
∂q
i
˙q, i =1,...,n. (4)
Define the residual vector r as
r = K
(α − τ − r)dt + p
, (5)
with (diagonal) K>0. The residual dynamics satisfies
˙r = −Kr + Kτ
c
,r(0) = 0, (6)
namely that of a linear exponentially stable system driven
by the (joint) contact torque τ
c
. Actually, for every com-
ponent of the residual dynamics we can write a transfer
function
r
i
(s)
τ
c,i
(s)
=
K
i
s + K
i
,i=1,...,n, (7)
having unitary gain. Note that eq. (7) is similar to the result
obtained in [8], where dynamic interaction torques between
robot joints, and thus the acceleration ¨q, are assumed to
be available. Instead, in order to be implemented, eq. (5)
requires proprioceptive measures (q, ˙q) only, the knowledge
of the current commanded input u, but no acceleration ¨q or
inversion of the inertia matrix M (q). For manipulators with
many d.o.f., the computation of the generalized momenta p
and vector α needed for the implementation of the residual
r in eq. (5) may need special care, e.g., by using recursive
schemes. A finite difference numerical approximation can
also be adopted for the inertial part in eq. (4), see [15].
During free motion, all residuals are practically zero.
The rising of one or more residuals above a fixed threshold
corresponds to the occurrence of a collision. In particular,
for large values of K
i
in eqs. (5–7), the evolution of
r
i
will reproduce accurately the evolution of the contact
torque τ
c,i
= J
T
ci
(q)F
c
, being J
ci
the i-th column of
q
1
d
1
d
2
F
c
F
c
q
2
x
1
y
1
x
0
y
0
x
2
y
2
Fig. 1. Possible collision forces in a two-link planar robot
the Jacobian matrix J
c
. Residuals rapidly return to zero
as contact is lost. As a result, this FDI-based scheme
allows the efficient detection of any robot collision without
external sensors. We note that the need of an accurate robot
dynamic model can be relaxed by developing an adaptive
version of the scheme. Moreover, a dynamic thresholding
of residuals can be used so as to avoid false detection due
to noise/additional disturbances [16].
III. I
DENTIFICATION OF CONTACT FORCES
Using our residual generator, it is immediate to identify
the robot link that has collided. In fact, assuming that the
robot is an open kinematic chain, if a collision occurs on
link k it is
r
i
(t) =0,i=1,...,k, r
j
(t)=0,i= k +1,...,n,
(8)
for the time interval of contact. Indeed, residuals will be
affected only by contact forces that perform work on robot
motion (not by those balanced by reaction forces in the
robot structure, i.e., belonging to the kernel of J
T
c
(q)).
While the distal residuals (i.e., beyond link k) will certainly
be unaffected, the sensitivity to cartesian generalized forces
F
c
of each proximal residual will vary in general with the
robot configuration (see also [17]).
By discussing a simple case in 2D, we show next
how to extract the largest possible information on impact
location and amplitude of the contact force in the case
of a single pointwise collision. The following analysis is
the dynamic counterpart of the static transformation from
cartesian forces to joint torques.
With reference to the two-link planar robot of Fig. 1, a
collision force F
c
=(F
cx
,F
cy
) may arise at point P
c1
on
the first link, at a distance d
1
from the first joint, or at point
P
c2
on the second link, at a distance d
2
from the second
joint. In the first situation, it is
τ
c1
= J
T
c1
(q)F
c
= J
T
c1
(q)
0
R
1
(q
1
)
1
F
c
=
d
1
1
F
cy
0
,
(9)
1000
where
0
R
i
is the rotation matrix from the base frame to
the i-th link frame. Equations (6) and (9) show that r
2
will
be unaffected (as expected) while r
1
will be driven only
by the force component normal to the link (scaled by d
1
).
It is clear that exact location and amplitude of the contact
force cannot be identified separately.
In the second situation, it is
τ
c2
= J
T
c2
(q)F
c
= J
T
c2
(q)
0
R
2
(q
1
,q
2
)
2
F
c
=
l
1
s
2
2
F
cx
+(l
1
c
2
+ d
2
)
2
F
cy
d
2
2
F
cy
,
(10)
and r
2
will always be affected only by the force normal to
the link (scaled by d
2
), while r
1
will be excited in general
by both the normal and the tangential components of the
contact force.
Equations (9–10) contain the largest information that
can be extracted from residual behavior, without external
sensing. Additional knowledge about the collision charac-
teristics may be used for further identification. For example,
if we assume that collision may occur only at the robot
end-effector, then d
2
= l
2
in eq. (10) is known and the
two components (
2
F
cx
,
2
F
cy
) can be generically identified
from eqs. (6) and (10). Alternatively, if the obstacles have
smooth contours and the collision point is ‘internal’ to the
second link, then
2
F
cx
≈ 0 (the link is tangential to the
obstacle) and eqs. (6) and (10) can be used to identify
both d
2
∈ (0,l
2
) and
2
F
cy
.
IV. H
YBRID FORCE/MOTION CONTROL
In this section we show how to exploit the information
on the contact force provided by the residual vector (5)
in order to devise a hybrid force/motion control scheme
that ensures motion of the contact point along the obstacle
surface and approximate regulation of the contact force.
As shown in Sect. III, the exact contact location and the
associated cartesian force cannot be independently identi-
fied in general. However, hybrid tasks can be accomplished
by suitably processing at the joint-space level the dynamic
effects of the robot-environment interaction. Performance
of the proposed control scheme will indeed depend on the
accuracy of the joint contact torque ‘measure’ provided
by the residual r, i.e., on the accuracy of the model and
on the input and measurement noise affecting the system.
Moreover, the bandwidth of the low-pass filters in eq. (7)
should be larger than that of the force and motion reference
signals. On the other hand, control tasks of practical interest
for robots in contact with unstructured environments are
usually performed at low velocities and require only a
rough regulation of interaction forces.
We consider only obstacles with smooth surfaces and
at most one pointwise contact between the robot and
the environment. Without loss of generality, we can take
K
i
= K, i =1,...,n, in eq. (7), so that vectors τ
c
and r
will have the same direction in IR
n
even during transients.
Finally, we assume that friction at the contact is negligible,
i.e., the contact force is directed along the normal to the
obstacle surface.
In standard hybrid force/motion control schemes, the
task space is decomposed into complementary direc-
tions, where only force and, respectively, motion is con-
trolled [18], [19]. Here, such decomposition is locally
performed in the joint velocity space, since the location and
orientation of the cartesian task frame Σ
c
is not known. In
particular, it is
τ
c
=
r
r
f =ˆr · f, (11)
where f = τ
c
. Moreover, the velocity of the contact point
between robot and environment is assumed to be tangent to
the obstacle surface
1
. As a result, the joint velocity vector
˙q during contact can be locally parametrized as
˙q = Ts, (12)
where s ∈ IR
n−1
and T is a n × (n − 1) matrix, whose
columns (normalized to unity) span the (n−1)-dimensional
subspace orthogonal to r, i.e., such that r
T
T =0.In
fact, although the location of the contact point on the
robotic structure is unknown, a motion of the cartesian
contact point along the obstacle surface corresponds to joint
displacements in the null space of r
T
(by the principle of
virtual work). Thus, the joint accelerations ¨q can be written
as
¨q = T ˙s +
˙
Ts ≈ T ˙s, (13)
where the last approximation is valid for ‘low velocity’ mo-
tion along the smooth surface of the obstacle. Substituting
eqs. (11) and (13) into model (1) yields
M(q)T −ˆr
˙s
f
+ c(q, ˙q)+g(q)=τ , (14)
where the n × n matrix M(q)=
M(q)T −ˆr
is
nonsingular by construction. Then, the nonlinear feedback
control law
τ = M(q)u + c(q, ˙q)+g(q), (15)
results in the linear and decoupled dynamics
˙s
f
= u =
u
s
u
f
. (16)
As a consequence, motion tracking for s and force regula-
tion for f can be executed by the linear control laws
u
s
=˙s
d
+ K
Ps
(s
d
− s)+K
Is
(s
d
− s)dt,
u
f
= f
d
+(K
Pf
− 1)(f
d
− f)+K
If
(f
d
− f)dt,
(17)
for positive gains K
Ps
, K
Is
, K
Pf
,andK
If
, and where
s
d
∈ IR
n−1
and f
d
∈ IR denote the desired (reference) be-
haviors for s and f , respectively. As concerns the ‘measure’
of the velocity and force parameters s and f to be used
in (17), according to the assumptions made in this section,
we can take f = r and s = T
#
˙q =(T
T
T )
−1
T
T
˙q.
1
In case of a compliant contact, the velocity in the normal direction is
still negligible for a quasi-static interaction between robot and obstacle.
1001
0.1 0 0.1 0.2 0.3 0.4 0.5 0.6
0.5
0.4
0.3
0.2
0.1
0
0.1
OBSTACLE
Fig. 2. Stroboscopic view of the robot during free and contact motion
(contact points are identified by stars)
V. S IMULATION RESULTS
In order to test the proposed collision detection and
hybrid force/motion control scheme, we have performed
numerical simulations for a 2R robot under gravity
2
.The
joint positions (q
1
,q
2
) are assumed to be measured by
digital encoders (with resolutions π/4096 and, respec-
tively, π/2048 [rad]), while joint velocities are obtained
by numerical differentiation. This measurement process
introduces a realistic level of noise in the simulation.
During free motion, the robot is being regulated to q
d
=
(π/2, 0) (q =(0, 0) is the downward free equilibrium con-
figuration) by a decoupling and linearizing state feedback
law (computed torque). At time t
c
, the second link collides
with a circular obstacle of radius ρ =0.2 [m] and centered
at C =(0.4, 0) [m] (see Fig. 2). The stiffness coefficient
of the robot-obstacle compliant contact is K
c
=50[N/m].
The behaviors of the residual r and of the joint velocity
˙q are shown in Figs. 3 and 4, respectively. Collision is de-
tected when at least one of the residual components exceeds
the threshold value 0.02. This time instant (t
c
=0.795 [s])
is indicated by the first vertical line shown in the figures.
The second vertical line corresponds to the time instant
where the controller is switched to the hybrid force/motion
law (15–17). This event is triggered at t
sw
=4.133 [s],
after the norm of the joint velocity has been constantly
below a fixed threshold (v
thres
=0.015 [rad/s]) for a given
time (∆t =0.175 [s]), i.e., when the assumption of quasi-
static interaction can be considered valid. Note that, under
regulation by computed torque control and for a compliant
contact, this condition is certainly attained in finite time.
In general, however, the transient behavior of contact
forces and joint velocities might result unsatisfactory just
after collision, thus requiring a suitable stability analysis
and control reconfiguration for the post-impact phase (not
2
The two links of the robot are thin rods with masses m
1
=0.193 and
m
2
=0.073 [kg], lengths l
1
=0.1492 and l
2
=0.381 [m], and inertias
I
1
=0.0015 and I
2
=1.949 · 10
−4
[kgm
2
]. The link centers of mass
are located on their axes, at a distance h
1
=0.1032 and, respectively,
h
2
=0.084 [m] from the joints.
0 2 4 6 8 10 12 14 16 18 20
0.12
0.1
0.08
0.06
0.04
0.02
0
0.02
s
Nm
r
1
r
2
Fig. 3. Residuals (free motion, collision, and hybrid control phases are
separated by vertical dash-dotted lines)
0 2 4 6 8 10 12 14 16 18 20
4
3
2
1
0
1
2
s
rad/s
q
1
.
.
q
2
.
.
Fig. 4. Joint velocities
needed in the present numerical case).
The (joint-space) control objectives of the hybrid
force/motion task are the regulation of f = τ
c
to the
desired value f
d
=0.1 [Nm], and the tracking of the
trajectory s
d
(t)=0.2 sin 0.2πt [rad/s] for the parameter s
in eq. (12). The control gains in eq. (17) are K
Ps
=2,
K
Is
=1, K
Pf
=0.01,andK
If
=0.1. Moreover,
K
1
= K
2
=50in eq. (5). The resulting behavior of
the controlled variables f and s are reported in Figs. 5
and 6, respectively. It can be seen that there is a good
tracking of the periodic motion, while the mean value of
the joint contact torque norm stabilizes at the desired level
(remember that the hybrid task starts only after t
sw
).
Although the hybrid control objectives can be imposed
only at the joint-space level, one is indeed interested in
the cartesian force/motion behavior at the contact. Figure 7
shows the actual cartesian normal force at the contact point
(which is unknown to the controller), while the angular
position of the contact point on the circular obstacle is
given in Fig. 8. Again, the periodic motion along the
obstacle tangent is performed at the correct frequency (with
1002
0 2 4 6 8 10 12 14 16 18 20
0
0.02
0.04
0.06
0.08
0.1
0.12
0.14
s
Nm
Fig. 5. Actual (blue, solid) and desired (red, dashed) norm f = τ
c
of contact torques at the joints
0 2 4 6 8 10 12 14 16 18 20
0.4
0.3
0.2
0.1
0
0.1
0.2
0.3
0.4
s
rad/s
Fig. 6. Actual (blue, solid) and desired (red, dashed) behavior of
parameter s (amplitude of joint velocity along the generalized contact
surface —see eq. (12))
an excursion of about 2πρ · (19/360) [m] on the contour),
while there is a sufficient limitation on the cartesian normal
force (the peak-to-peak variation is less than 40% of the
average value).
Finally, the overall behavior and limited amplitude of
the control torques τ in Fig. 9 indicate that the transition
between free motion, collision, and hybrid force/motion
task phases occur in a rather smooth way. As a result, it
would be hard to recognize the collision by considering
only the control torque profiles rather than the residuals.
VI. C
ONCLUSIONS
Residual signals generated by a model-based fault di-
agnostic scheme can be used as a sensorless and efficient
collision detection method for robot manipulators moving
in an unstructured environment. The residual vector allows
to identify the robot link that has collided and, to some
extent, the contact force location and intensity. Moreover,
the information content in the residual dynamics can be
used for designing a hybrid force/motion controller that
0 2 4 6 8 10 12 14 16 18 20
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
s
N
Fig. 7. Norm of the cartesian force at the contact point (normal to the
obstacle)
0 5 10 15 20 25 30 35 40
144
142
140
138
136
134
132
130
128
126
124
s
deg
Fig. 8. Angular motion of the contact point on the obstacle contour
handles the interaction task which follows the detected
collision. The novel feature of this controller is that it
works on a suitably defined decomposition of joint-space
variables, since the actual location and orientation of the
cartesian contact frame is not known explicitly. At least for
compliant obstacles, reasonable cartesian performance can
still be achieved, e.g., in terms of following a trajectory
on the (unknown and not sensed) obstacle surface while
keeping the contact force in the neighborhood of a specified
value (without direct force sensing).
The preliminary results in this paper deserve further
analysis and experimental verification. On one side, it
is reasonable that the backdrivability of cartesian contact
forces into their joint-space counterparts (as in direct-
drive robots) increases the sensitivity of the sensorless
collision detection method and of the associated hybrid
force/motion controller. On the other hand, safety indices
related to collisions are typically improved by the presence
of some compliance in the robot structure. Interestingly, the
proposed collision detection method can be easily extended
also to the class of robots with joint transmission elasticity.
1003
