# US 10,478,969 B2 · Method for the improved detection of the collision of a robot with its environment, system and computer program product implementing said method

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 10,478,969 B2
- Document ID: US10478969B2
- Status: Granted
- Kind: B2
- Application number: 14/766,568
- Filed: 2014-02-07
- Published: 2016-01-07
- Granted: 2019-11-19
- Earliest claimed: 2013-02-14
- Assignees: Commissariat A L'energie Atomique Et Aux Energies Alternatives
- Applicants: Commissariat A L'energie Atomique Et Aux Energies Alternatives
- Inventors: Alex Caldas; Maria Makarov; Mathieu Grossard
- Web page: https://patentgrep.com/patent/US10478969B2

## CPC

- B25J 9/1674: {characterised by safety, monitoring, diagnostic}
- B25J 9/06: characterised by multi-articulated arms
- B25J 9/1605: {Simulation of manipulator lay-out, design, modelling of manipulator}
- B25J 9/1676: {Avoiding collision or forbidden zones}
- G01P 15/001: {by measuring acceleration changes by making use of a triple differentiation of a displacement signal}
- G05B 2219/40201: Detect contact, collision with human
- Y10S 901/09: Closed loop, sensor feedback controls arm movement
- Y10S 901/46: Sensing device
- B25J: MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES ({manipulators specially adapted for use in surgery A61B34/70; manipulators used in cleaning hollow articles B08B9/04}; manipulators associated with rolling mills B21B39/20; manipulators associated with forging machines B21J13/10; {manipulators associated with picking-up and placing mechanisms B23P19/007}; means for holding wheels or parts thereof B60B30/00; {vehicles with ground-engaging propulsion means, e.g. walking members B62D57/02, B62D57/032; devices for picking-up and depositing articles or materials between conveyors B65G47/90, B65G47/91; manipulators with gripping or holding means for transferring packages B65H67/065}; cranes B66C; {manipulators used in the protection or supervision of pipe-line installations F17D5/00; walking equipment adapted for nuclear steam-generators F22B37/006}; manipulators specially adapted for, or associated with, nuclear reactors G21C; {apparatus used for handling wafers during manufacture or treatment of semiconductor H10P72/50})
- G01P: MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT (measuring or recording blood flow A61B5/02, A61B8/06; monitoring speed or deceleration of electrically-propelled vehicles B60L3/00; vehicle lighting systems adapted to indicate speed B60Q1/54; determining position or course in navigation, measuring ground distance in geodesy or surveying G01C; combined measuring devices for measuring two or more variables of movement G01C23/00; measuring velocity of sound G01H; measuring velocity of light G01J7/00; measuring direction or velocity of solid objects by reception or emission of radiowaves or other waves and based on propagation effects, e.g. Doppler effect, propagation time, direction of propagation, G01S; measuring speed of nuclear radiation G01T; measuring acceleration of gravity G01V; {measuring or recording the speed of trains B61L23/00; speed indicators incorporated in motor vehicles B60K35/00; measuring frequency or phase G01R; traffic control G08G})
- G05B: CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS (systems for controlling or regulating non-electric variables G05D; systems for regulating electric or magnetic variables G05F; control devices or systems insofar as characterised by mechanical features only G05G)
- Y10S: TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS

## Abstract

A method of detecting collision between a robot and its environment comprises: generating, on the basis of a dynamic model of the robot, a signal representative of the collisions between the robot and environment, the signal being a residual r; carrying out an adaptive high-pass filtering of the residual r; determining, in a recursive manner, an adaptive threshold T composed of at least one first dynamic term TΔ1 equal to an item of information regarding the parametric uncertainties between the model and real behavior of the robot, the parametric uncertainties being related to a first variable characteristic of the state of the robot among the following variables: position, velocity or acceleration of an articulation of the robot, or a linear or nonlinear function of one of these variables or of a combination; comparing the filtered residual with the adaptive threshold T to deduce the existence or otherwise of a collision.

## Claims

### Claim 1

A method for detecting a collision between a robot composed of a plurality of bodies linked together by at least one articulation and the environment of the robot, said method being executed by a system for detecting a collision comprising a processor, a control member, and a physical interface, the method comprising the steps of:
moving at least one articulation of the robot by manipulating, via a motor of the robot, a torque applied to the at least one articulation of the robot,
receiving, from a measurement device of the control member, at least a measure of a variable characteristic of the state of the robot among a position, a velocity or an acceleration of the at least one articulation of the robot,
executing, with the processor, a computer program stored on a non-transitory computer readable medium, the computer program stored on the non-transitory computer readable medium including instructions that when executed by the processor cause the processor to automatically perform the steps of:
generating, from a dynamic model of the robot, a signal representative of the collisions between the robot and the environment of the robot, said signal being called a residual r and comprising as many components as articulations of the robot,
adaptive high-pass filtering of the residual r so as to render the residual r independent of parametric or non-parametric uncertainties related to low-frequency phenomena,
determining recursively adaptive thresholds T composed of at least one first dynamic term TΔ1 dependent on the parametric uncertainties between said model and the real behavior of the robot, said parametric uncertainties being related to a first variable e1 characteristic of the state of said robot from among the following variables: the position, the velocity or the acceleration of the at least one articulation of the robot, or a linear or nonlinear function of one of these variables or of a combination of these variables, wherein determining recursively an adaptive threshold T of the adaptive thresholds T at a given instant is based upon a previously determined adaptive threshold of the adaptive thresholds T at an instant previous to the given instant, so that the adaptive threshold T is dependent on said parametric uncertainties between said model and the real behavior of the robot,
comparing the filtered residual with the adaptive threshold T so as to deduce therefrom the existence or otherwise of the collision,
deciding, as a result of the comparison of the filtered residual and the adaptive threshold T, if a collision between the robot and the environment of the robot occurs,
providing the decision to the interface, and
utilizing the decision provided to the interface, wherein
the adaptive high-pass filtering is implemented via a recursive least squares algorithm and comprises:
estimating, in a recursive manner, the coefficients of the transfer function of the high-pass filter G0−1, and
filtering the residual r with the estimated high-pass filter.

### Claim 2 (depends on 1)

The method of detecting collision as claimed in claim 1, wherein said adaptive threshold T is composed of the sum of several dynamic terms TΔ1, TΔ2, TΔ3, each equal to an item of information regarding the parametric uncertainties between said model and the real behavior of the robot, said parametric uncertainties being related to a different variable, characteristic of the state of said robot from among the following variables: the position, the velocity or the acceleration of a fixed point of the robot, or a linear or nonlinear function of one of these variables or of a combination of these variables.

### Claim 3 (depends on 1)

The method of detecting collision as claimed in claim 1, in which said adaptive threshold T furthermore comprises a static term Tstatic configured so as to be greater than a measurement noise level.

### Claim 4 (depends on 1)

The method of detecting collision as claimed in claim 1, in which an additional step of temporal filtering is applied to the filtered residual for each of the components.

### Claim 5 (depends on 4)

The method of detecting collision as claimed in claim 4, in which the additional step of temporal filtering is a step of root mean square (RMS) calculation.

### Claim 6 (depends on 1)

The method of detecting collision as claimed in claim 1, in which the step of determining at least one first dynamic term TΔ1, TΔ2, TΔ3 of the adaptive threshold T is carried out by means of a recursive least squares algorithm.

### Claim 7 (depends on 6)

The method of detecting collision as claimed in claim 6, in which an additional step of temporal filtering is applied to said dynamic term TΔ1, TΔ2, TΔ3 of the adaptive threshold T for each of its components.

### Claim 8 (depends on 7)

The method of detecting collision as claimed in claim 7, in which the additional step of temporal filtering is a step of root mean square calculation.

### Claim 9 (depends on 1)

The method of detecting collision as claimed in claim 1, in which the step of determining at least one first dynamic term TΔ1, TΔ2, TΔ3 of the adaptive threshold T comprises the following sub-steps:
estimating, in a recursive manner, the coefficients of the transfer function Δi modeling the parametric uncertainties related to said first variable ei characteristic of the state of said robot, and
filtering said first variable ei characteristic of the state of said robot with a filter of transfer function Δi estimated in the previous step so as to obtain a dynamic term TΔ1, TΔ2, TΔ3 of the adaptive threshold T.

### Claim 10 (depends on 1)

The method of detecting collision as claimed in claim 1, in which step of comparing the filtered residual with the adaptive threshold T so as to deduce therefrom the existence or otherwise of a collision comprises the following sub-steps:
comparing, for each articular component, the filtered residual with the adaptive threshold T, and
concluding the existence of a collision if, for at least K components, the filtered residual is greater than the adaptive threshold T, K being a strictly positive predetermined integer less than or equal to the number of articulations of the robot.

### Claim 11 (depends on 1)

The method of detecting collision as claimed in claim 1, in which step of generating the residual consists of the following sub-steps:
determining, on the basis of an item of information regarding the state of the robot and by way of a dynamic model, an estimation of the articular torques of the robot,
performing a measurement of the state of the robot, for example a measurement of the articular torques, and
calculating the residual as the difference between the estimation and the measurement of the state of the robot.

### Claim 12 (depends on 1)

The method of detecting collision as claimed in claim 1, in which the parametric uncertainties related to the articular acceleration of the robot are uncertainties regarding the inertia matrix of the robot.

### Claim 13 (depends on 1)

The method of detecting collision as claimed in claim 1, in which the parametric uncertainties related to the articular velocity of the robot are uncertainties regarding the matrix of the centrifugal and Coriolis terms of the robot and/or regarding the viscous frictions.

### Claim 14 (depends on 1)

The method of detecting collision as claimed in claim 1, in which a nonlinear function is the sign function or the exponential function or the absolute value function.

### Claim 15 (depends on 14)

The method of detecting collision as claimed in claim 14, wherein the parametric uncertainties related to the sign of the articular velocity of the robot are uncertainties regarding the dry frictions.

### Claim 16

A system for command of a robot comprising:
a control member for the manipulation of the robot;
an interface for the exchange of information regarding the state of the robot; and
a processor for detecting collisions which is configured to execute a method for detecting a collision between a robot composed of a plurality of bodies linked together by at least one articulation and the environment of the robot, said method comprising:
moving the at least one articulation of the robot by manipulating, via a motor of the robot, a torque applied to the at least one articulation of the robot,
receiving, from a measurement device of the control member, at least a measure of a variable characteristic of the state of the robot among a position, a velocity or an acceleration of the at least one articulation of the robot,
executing, with the processor, a computer program stored on a non-transitory computer readable medium, the computer program stored on the non-transitory computer readable medium including instructions that when executed by the processor cause the processor to automatically perform the steps of:
generating, from a dynamic model of the robot, a signal representative of the collisions between the robot and the environment of the robot, said signal being called a residual r and comprising as many components as articulations of the robot,
adaptive high-pass filtering of the residual r so as to render the residual r independent of parametric or non-parametric uncertainties related to low-frequency phenomena,
determining recursively adaptive thresholds T composed of at least one first dynamic term TΔ1 dependent on the parametric uncertainties between said model and the real behavior of the robot, said parametric uncertainties being related to a first variable e1 characteristic of the state of said robot from among the following variables: the position, the velocity or the acceleration of the at least one articulation of the robot, or a linear or nonlinear function of one of these variables or of a combination of these variables, wherein determining recursively an adaptive threshold T of the adaptive thresholds T at a given instant is based upon a previously determined adaptive threshold of the adaptive thresholds T at an instant previous to the given instant, so that the adaptive threshold T is dependent on said parametric uncertainties between said model and the real behavior of the robot,
comparing the filtered residual with the adaptive threshold T so as to deduce therefrom the existence or otherwise of the collision,
deciding, as a result of the comparison of the filtered residual and the adaptive threshold T, if a collision between the robot and the environment of the robot occurs,
providing the decision to the interface, and
utilizing the decision provided to the interface, wherein
the adaptive high-pass filtering is implemented via a recursive least squares algorithm and comprises:
estimating, in a recursive manner, the coefficients of the transfer function of the high-pass filter G0−1, and
filtering the residual r with the estimated high-pass filter.