collision detection and robot reaction

Robotics 2

Prof. Alessandro De Luca

Collision detectionand robot reaction

Handling of robot collisionsn safety in physical Human-Robot Interaction (pHRI)n robot dependability (i.e., beyond reliability)

n mechanics: lightweight construction and inclusion of compliancen next generation with variable stiffness actuation devicesn typically, more/additional exteroceptive sensing neededn human-oriented motion planning (“legible” robot trajectories) n control strategies with safety objectives/constraints

n prevent, avoid, detect and react to collisionsn possibly, using only robot proprioceptive sensors

n different phases in the collision event pipeline

PHRIENDSFP6 STREP(2006-09)

Robotics 2 2

SAPHARI FP7 IP

(2011-15) SYMPLEXITYH2020 IP (2015-18)

European projects that have funded our research developments

Collision event pipeline

Robotics 2 3

S. Haddadin, A. De Luca, A. Albu-Schäffer: “Robot Collisions: A Survey on Detection, Isolation, and Identification,” IEEE Trans. on Robotics, vol. 33, no. 6, pp. 1292-1312, 2017

Collision detection in industrial robotsn advanced option available for some robots (ABB, KUKA, UR ...)n allow only detection, not isolation

n based on large variations of control torques (or motor currents)

n based on comparison with nominal torques on a desired trajectory

n based on robot state and numerical estimate of acceleration

n based on the parallel simulation of robot dynamics

n sensitive to actual control law and reference trajectoryn require noisy acceleration estimates or on-line inversion of the

robot inertia matrixRobotics 2 4

⇔𝜏 𝑡$ − 𝜏(𝑡$'() ≥ 𝜀 𝜏, 𝑡$ − 𝜏,(𝑡$'() ≥ 𝜀,, for at least one joint 𝑖

⇒𝜏0 = 𝑀 𝑞0 �̈�0 + 𝑆 𝑞0, �̇�0 �̇�0 + 𝑔 𝑞0 + 𝑓(𝑞0, �̇�0) 𝜏 − 𝜏0 ≥ 𝜀

⇒ ⇒𝜏: = 𝑀 𝑞 �̈�: + 𝑆 𝑞, �̇� �̇� + 𝑔 𝑞 + 𝑓(𝑞, �̇�) 𝜏 − 𝜏: ≥ 𝜀�̈�: =𝑑�̇�𝑑𝑡

⇒�̈�< = 𝑀'( 𝑞 𝜏 − 𝑆 𝑞, �̇� �̇� − 𝑔 𝑞 − 𝑓(𝑞, �̇�) �̇� − �̇�< ≥ 𝜀=̇, 𝑞 − 𝑞< ≥ 𝜀=

ABB collision detectionn ABB IRB 7600 video

n the only feasible robot reaction is to stop!Robotics 2 5

n robot model with (possible) collisions

n collisions may occur at any (unknown) location along the whole robotic structure

n simplifying assumptions (some may be relaxed)n manipulator is an open kinematic chainn single contact/collisionn negligible friction (or has to be identified and used in the model)

Collisions as system faults

with transpose of the Jacobianassociated to the contact point/area

control torque

joint torque caused by link collisioninertiamatrix

Coriolis/centrifugal(with “good” factorization):�̇� − 2𝑆 is skew-symmetric

Robotics 2 6

Analysis of a collision

in dynamic conditions a contact force/torqueon 𝑖th link produces

accelerationsat ALL joints

in static conditions a contact force/torqueon 𝑖th link is balanced

ONLY by torques at preceding joints 𝑗 ≤ 𝑖

Robotics 2 7

Relevant dynamic properties

n total energy and its variation

n generalized momentum and its decoupled dynamics

using the skew-symmetric property

NOTE: this is a vector versionof the same formula already encountered in actuator FDI

Robotics 2 8

Ex: prove this expression!

Monitoring collisions

Normal Modeof Operation

CollisionMonitor

Detection Isolation

Collision recognized

σ

|σ| ≥ σcoll

∥r∥ ≥ rcoll

r

NO

NOYES

YES

τ

without externalor contact sensors

usedeactivate/activate

continue

Robotics 2 9

.q, q

Reaction Strategy

Energy-based detection of collisions

n scalar residual (computable)

n … and its dynamics (needed only for analysis)

a stable first-order linear filter, excited by a collision!

Robotics 2 10

also via N-E algorithm!

Block diagram of residual generatorenergy-based scalar signal

Robotics 2 11

robot ∫+

+

∫+

−

++

scalarresidual generator

initializationof integratorB𝜎 0 = 𝐸(0)(zero if robotstarts at rest)

rigid robot with possible extra torque due to collision

B𝜎 0

Analysis of the energy-based methodn very simple scheme (scalar signal) n it can only detect the presence of collision

forces/torques (wrenches) that produce work on the linear/angular velocities (twists) at the contact

n does not succeed when the robot stands still…

Robotics 2 12

Collision detectionsimulation with a 7R robot

detection of a collision with a fixed obstacle in the work spaceduring the execution of a Cartesian trajectory (redundant robot)

signal back to zero after contact is over

Robotics 2 13

obstacle

Cartesian path

contact detectedwhen exceeding

a threshold

Collision detection experiment with a 6R robot

robot at rest or moving under Cartesian impedance control

on a straight horizontal line(with a F/T sensor at wrist for analysis)

Robotics 2 14

6 phases A: contact force applied is acting against

motion direction ⇒ detectionB: no force applied, with robot at restC: force increases gradually, but robot is

at rest ⇒ no detectionD: robot starts moving again, with force

being applied ⇒ detectionE: robot stands still and a strong force is

applied in 𝑧-direction ⇒ no detectionF: robot moves, with a 𝑧-force applied

≈ orthogonal to motion direction ⇒poor detection

Momentum-based isolation of collisions

n residual vector (computable)

n … and its decoupled dynamics

(diagonal)

𝑁 first-order, linear filters with unitary gains, excited by a collision!(all residuals go back to zero if there is no longer contact = post-impact phase)Robotics 2 15

in case, needs modified N-E algorithm!

Block diagram of residual generatormomentum-based vector signal

Robotics 2 16

robot ∫+

+

∫+

−

+ ++

residualgenerator

rigid robot with possible extra torque due to collision

�̂� 0initializationof integrators�̂� 0 = 𝑝(0)(zero if robotstarts at rest)

Analysis of the momentum method

n residual vector contains directional information on the torque at the robot joints resulting from link collision (useful for robot reaction in post-impact phase)

n ideal situation (no noise/uncertainties)

n isolation property: collision has generically occurred in an area located up to the 𝑖th link if

Robotics 2 17

Safe reaction to collisions

Normal Modeof Operation

CollisionMonitor

Collision recognized

Reaction Strategy

σ r

NOYES use

deactivatere-activate

continue

internal robot state and control command

τ

without external or contact sensors

𝝉𝑹

Robotics 2 18

Robot reaction strategy

n upon detection of a collision ( is over some threshold)n no reaction (strategy 0): robot continues its planned motion…n stop robot motion (strategy 1): either by braking or by

stopping the motion reference generator and switching to a high-gain position control law

n reflex* strategy: switch to a residual-based control law

n “zero-gravity” control in any operative mode

“joint torque command in same direction of collision torque ”(diagonal)

* = in robots with transmission/joint elasticity, the reflex strategy can be implemented in different ways (strategies 2, 3, 4)

Robotics 2 19

Analysis of the reflex strategy

“a lighter robot that can be easily pushed way”

n in ideal conditions, this control strategy is equivalent to a reduction of the effective robot inertia as seen by the collision force/torque

from a cow … ... to a frog!

Robotics 2 20

DLR LWR-III robot dynamicsn lightweight (14 kg) 7R anthropomorfic robot with

harmonic drives (elastic joints) and joint torque sensors

n proprioceptive sensing: motor positions and joint elastic torques

motor torques commands joint torques due to linkcollision

elastic torques at the joints

Robotics 2 21

friction at link side is negligible!

Exploded joint of LWR-III robot

Robotics 2 22

source of joint elasticity

Collision isolation for LWR-III robot elastic joint case

§ few alternatives for extending the rigid case results§ for collision isolation, the simplest one takes advantage of the

presence of joint torque sensors“replace the commanded torque to the motorswith the elastic torque measured at the joints”

n other alternatives use § link+motor position measures ⇒ needs knowledge also of joint stiffness K§ link+motor momentum + commanded torque ⇒ affected by motor friction

n motion control is more complex in the presence of joint elasticity n different active strategies of reaction to collisions are possible

Robotics 2 23

Control of DLR LWR-III robot elastic joint case

n general control law using full state feedback(motor position and velocity, joint elastic torque and its derivative)

n “zero-gravity” condition is realized only in a (quasi-static) approximate way, using just motor position measures

motorposition

error

elastic jointtorque ffwcommand

elastic jointtorque error

(diagonal) matrixof joint stiffness

motorposition

linkposition

Robotics 2 24

n strategy 2: floating reaction (robot ≈ in “zero-gravity”)

n strategy 3: reflex torque reaction (closest to the rigid case)

n strategy 4: admittance mode reaction (residual is used as the new reference for the motor velocity)

n further possible reaction strategies (rigid or elastic case) n based on impedance controln sequence of strategies (e.g., 4 + 2)n time scaling: stop/reprise of reference trajectory, keeping the pathn Cartesian task preservation (exploits robot redundancy by projecting

reaction torque in a task-related dynamic null space)

Reaction strategiesspecific for elastic joint robots

Robotics 2 25

Experiments with LWR-III robot “dummy” head

dummy head equippedwith an accelerometer

robot straighten horizontally,mostly motion of joint 1 @30o/sec

Robotics 2 26

Dummy head impact

strategy 2: floating reactionstrategy 0: no reactionplanned trajectory ends just afterthe position of the dummy head

impact velocity is rather low here andthe robot stops switching to float mode

Robotics 2 27

videovideo

Delay in collision detection

measured (elastic)joint torqueresidual r1

0/1 index fordetectiondummy head acceleration

impact withthe dummy head

2-4 msec!

gain KI = diag{25}

threshold = 5-10% ofmax rated torque

Robotics 2 28

Experiments with LWR-III robot balloon impact

possibility of repeatablecomparison of different

reaction strategies at high speed conditions

Robotics 2 29

Balloon impact

coordinatedjoint motion@90o/sec

strategy 4: admittance mode reaction

Robotics 2 30

video

Experimental comparison of strategiesballoon impact

n residual and velocity at joint 4 with various reaction strategies

impact at 90o/sec with coordinated joint motion

0: no reaction

1: motion stop

4: the fastest in stopping the robot

in post-impact phase

Robotics 2 31

Human-Robot Interaction ⎯ 1

n first impact @60o/sec

strategy 4: admittance mode strategy 3: reflex torque

Robotics 2 32

videovideo

Human-Robot Interaction ⎯ 2

n first impact @90o/sec

strategy 3: reflex torqueRobotics 2 33

video

“Portfolio” of reaction strategiesresidual amplitude ∝ severity level of collision

Stop Reflex Preserve

Cartesian trajectory(use of redundancy)

RepriseReaction

Taskrelaxation

Cartesian path(time scaling)

all transitions are controlled by

suitable thresholds

on the residuals

Robotics 2 34

Experiments with LWR-III robot time scaling

n robot is position-controlled (on a given geometric path)n timing law slows down, stops, possibly reverses (and then reprises)

Robotics 2 35

Reaction with time scaling

video

Robotics 2 36

Use of kinematic redundancyn collision detection ⇒ robot reacts so as to preserve as

much as possible (and if possible at all) execution of the planned Cartesian trajectory for the end-effector

Robotics 2 37

instant of collision detection

Task kinematics

§ task coordinates with (redundancy)

§ (all) generalized inverses of the task Jacobian

§ all joint accelerations realizing a desired task acceleration (at a given robot state)

arbitrary jointacceleration

Robotics 2 38

Dynamic redundancy resolution

§ all joint torques realizing a precise control of the desired (Cartesian) task

set for compactness

for any generalized inverse G, the joint torque has two contributions: one imposes the task acceleration control, the other does not affect it

arbitrary joint torqueavailable for reaction to collisions

projection matrix in the dynamic null space of J

Robotics 2 39

Dynamically consistent solutioninertia-weighted pseudoinverse

§ the most natural choice for matrix G is to use the dynamically consistent generalized inverse of J

§ in a dual way, denoting by H a generalized inverse of JT, the joint torques can in fact be always decomposed as

§ the inertia-weighted choices for H and G are then

§ thus, the dynamically consistent solution is given by

Robotics 2 40

Cartesian task preservation

n wish to preserve the whole Cartesian task (end-effector position & orientation)reacting to collisions by using only self-motions in the joint space

n if the residual (∝ contact force) grows too large, orientation is relaxed first and then, if necessary, the full task is abandoned (priority is given to safety)

spherical obstacle

simulation in Simulinkvisualization in VRML

De Luca, Ferrajoli @IROS 2008

video

Robotics 2 41

Cartesian task preservationExperiments with LWR4+ robot

video @IROS 2017

Robotics 2 42idle ⇔ relax ⇔ abort

Combined use6D F/T sensor at the wrist + residuals

n enables easy distinction of intentional interactions vs. unexpected collisionsn it is sufficient to include the F/T measure in the expression of the residual!Robotics 2 43

HRI/HRC in closed control architecturesKUKA KR5 Sixx R650 robot

Robotics 2 44

§ low-level control laws are not known nor accessible by the user: no current or torque commands can be used

§ user programs, based also on other exteroceptive sensors (vision, Kinect, F/T sensor) can be implemented on an external PC via the RSI (RobotSensorInterface), communicating with the KUKA controller every 12 ms

§ robot measures available to the user: joint positions (by encoders) and [absolute value of] motor currents

§ controller reference is given as a velocity or a position in joint space (also Cartesian commands are accepted)

motor currents measured on first three joints

Collision detection and stop

Robotics 2 45

high-pass filtering of motor currents (a signal-based detection...)

video @ICRA 2013

Distinguish accidental collisions fromintentional contact and then collaborate

Robotics 2 46

with both high-pass and low-pass filtering of motor currents － here collaboration mode is manual guidance of the robot

video @ICRA 2013

Other possible robot reactionsafter collaboration mode is established

Robotics 2 47

video@ICRA2013

collaboration mode:pushing/pulling

the robot

collaboration mode:compliant-likerobot behavior

video@ICRA2013

Bibliography

Robotics 2 48

n A. De Luca and R. Mattone, “Sensorless robot collision detection and hybrid force/motion control,” IEEE Int. Conf. on Robotics and Automation, pp. 999-1004, 2005.

n A. De Luca, A. Albu-Schäffer, S. Haddadin, and G. Hirzinger, “Collision detection and safe reaction with the DLR-III lightweight manipulator arm,” IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 1623-1630, 2006.

n S. Haddadin, A. Albu-Schäffer, A. De Luca, and G. Hirzinger, “Collision detection and reaction: A contribution to safe physical human-robot interaction,” IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 3356-3363, 2008 (IROS 2008 Best Application Paper Award).

n A. De Luca and L. Ferrajoli, “Exploiting robot redundancy in collision detection and reaction,” IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 3299-3305, 2008.

n A. De Luca and L. Ferrajoli, “A modified Newton-Euler method for dynamic computations in robot fault detection and control,” IEEE Int. Conf. on Robotics and Automation, pp. 3359-3364, 2009.

n S. Haddadin, “Towards Safe Robots: Approaching Asimov’s 1st Law,” PhD Thesis, Univ. of Aachen, 2011.

n M. Geravand, F. Flacco, and A. De Luca, “Human-robot physical interaction and collaboration using an industrial robot with a closed control architecture,” IEEE Int. Conf. on Robotics and Automation, pp. 3985-3992, 2013.

n E. Magrini, A. De Luca, “Human-robot coexistence and contact handling with redundant robots,” IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 4611-4617, 2017.

n S. Haddadin, A. De Luca, and A. Albu-Schäffer, “Robot collisions: A survey on detection, isolation, and identification,” IEEE Trans. on Robotics, vol. 33, no. 6, pp. 1292-1312, 2017.