Collision handling by a robot

Abstract

The invention relates to a method of collision handling for a robot with a kinematic chain structure comprising at least one kinematic chain, wherein the kinematic chain structure includes: a base, links, joints connecting the links, actuators and at least one end-effector, a sensor S.sub.distal.i in the most distal link of at least one of the kinematic chains for measuring/estimating force/torque, and sensors S.sub.i for measuring/estimating proprioceptive data, wherein the sensors S.sub.i are arbitrarily positioned along the kinematic chain structure, the method including: providing a model describing the dynamics of the robot; measuring and/or estimating with sensor S.sub.distal.i force/torque F.sub.ext,S.distal.i in the most distal link of at least one of the kinematic chains; measuring and/or estimating with the sensors S.sub.i proprioceptive data: base and robot generalized coordinates q(t) and their time derivative {dot over (q)}(t), generalized joint motor forces τ.sub.m, external forces F.sub.S, a base orientation φ.sub.B(t) and a base velocity {dot over (x)}(t).sub.B; generating an estimate {circumflex over (τ)}.sub.∈ of the generalized external forces τ.sub.ext with a momentum observer based on at least one of the proprioceptive data and the model; generating an estimate {umlaut over ({circumflex over (q)})}(t) of a second derivative of base and robot generalized coordinates {umlaut over (q)}(t), based on {circumflex over (τ)}.sub.∈ and τ.sub.m; estimating a Cartesian acceleration {umlaut over ({circumflex over (x)})}.sub.D of point D on the kinematic chain structure based on {umlaut over ({circumflex over (q)})}(t); compensating the external forces F.sub.D for rigid body dynamics effects based on {umlaut over ({circumflex over (x)})}.sub.D and for gravity effects to obtain an estimated external wrench {circumflex over (F)}.sub.ext,S.i; compensating {circumflex over (τ)}.sub.∈ for the Jacobian J.sub.S.distal.i.sup.T transformed F.sub.ext,S.distal.i to obtain an estimation {circumflex over (τ)}.sub.ext,col of generalized joint forces originating from unexpected collisions; detecting a collision based on given thresholds τ.sub.thresh and F.sub.S.i,thresh if {circumflex over (τ)}.sub.ext,col>τ.sub.thresh and/or if {circumflex over (F)}.sub.ext,S.i>F.sub.S.i,thresh.

Claims

1. A method of collision handling for a robot with a kinematic chain structure comprising at least one kinematic chain, wherein the kinematic chain structure comprises a base, links, joints connecting the links, actuators and at least one end-effector, a sensor S.sub.distal.i in a most distal link of at least one of the kinematic chains for measuring/estimating force/torque, and sensors S.sub.i for measuring/estimating proprioceptive data, wherein the sensors S.sub.i are arbitrarily positioned along the kinematic chain structure, the method comprising: providing a model describing dynamics of the robot; measuring and/or estimating with sensor S.sub.distal.i force or torque F.sub.ext,S.distal.i in the most distal link of at least one of the kinematic chains; measuring and/or estimating with the sensors S.sub.i proprioceptive data: base and robot generalized coordinates q(t) and their time derivative {dot over (q)}(t), generalized joint motor forces τ.sub.m, external forces F.sub.S, a base orientation φ.sub.B(t) and a base velocity {dot over (x)}(t).sub.B; generating an estimate {circumflex over (τ)}.sub.∈ of generalized external forces τ.sub.ext with a momentum observer based on at least one of the proprioceptive data and the model; generating an estimate {umlaut over ({circumflex over (q)})}(t) of a second derivative of base and robot generalized coordinates {umlaut over (q)}(t), based on {circumflex over (τ)}.sub.∈ and τ.sub.m; estimating a Cartesian acceleration {umlaut over ({circumflex over (x)})}.sub.D of a point D on the kinematic chain structure based on {umlaut over ({circumflex over (q)})}(t); compensating F.sub.S for rigid body dynamics effects based on {umlaut over ({circumflex over (x)})}.sub.D and for gravity effects to obtain an estimated external wrench {circumflex over (F)}.sub.ext,S.i; compensating {circumflex over (τ)}.sub.∈ for a Jacobian J.sub.S.distal.i.sup.T transformed F.sub.ext,S.distal.i to obtain an estimation {circumflex over (τ)}.sub.ext,col of generalized joint forces originating from unexpected collisions; and detecting a collision based on given thresholds τ.sub.thresh and F.sub.S.i,thresh if τ.sub.ext,col>τ.sub.thresh and/or if {circumflex over (F)}.sub.ext,S.i>F.sub.S.i,thresh.

2. The method according to claim 1, further comprising generating recursively compensation wrenches for compensating the external forces F.sub.S for rigid body dynamics effects based on {umlaut over ({circumflex over (x)})}.sub.D and for gravity effects to obtain an estimated external wrench {circumflex over (F)}.sub.ext,S.i, if multiple sensors S are attached to one of the joints or links.

3. The method according to claim 1, further comprising determining contact locations r.sub.C of collisions with the kinematic chain structure by calculating a line of force action r.sub.d+λf.sub.i/∥f.sub.i∥ with r.sub.d=(S.sup.T (f.sub.i)).sup.#m.sub.i of the collision and by intersecting the line of force action with the kinematic chain structure geometry.

4. The method according to claim 3, further comprising: determining full contact Jacobians J.sub.C.i=J.sub.c,iJ.sub.i based on the determined contact locations r.sub.C; and determining the external wrenches (F.sub.ext,1.sup.T . . . F.sub.ext,n.sup.T)=(J.sub.C.1.sup.T . . . J.sub.C.n.sup.T).sup.#τ.sub.ext,col.

5. The method according to claim 4, further comprising controlling the robot dependent on r.sub.C and (F.sub.ext,1.sup.T . . . F.sub.ext,n.sup.T).sup.T.

6. The method according to claim 1, wherein the robot is a humanoid robot.

7. A robot capable of collision handling, the robot comprising a kinematic chain structure comprising at least one kinematic chain, wherein the kinematic chain structure comprises a base, links, joints connecting the links, actuators and at least one end-effector, a sensor S.sub.distal.i in a most distal link of at least one of the kinematic chains for measuring/estimating force/torque, and sensors S.sub.i for measuring/estimating proprioceptive data, wherein the sensors S.sub.i are arbitrarily positioned along the kinematic chain structure, the robot designed and set up to: provide a model describing dynamics of the robot; measure and/or estimate with sensor S.sub.distal.i force or torque F.sub.ext,S.distal.i in the most distal link of at least one of the kinematic chains; measure and/or estimate with the sensors S.sub.i proprioceptive data: base and robot generalized coordinates q(t) and their time derivative {dot over (q)}(t), generalized joint motor forces τ.sub.m, external forces F.sub.S, a base orientation φ.sub.B(t) and a base velocity {dot over (x)}(t).sub.B; generate an estimate {circumflex over (τ)}.sub.∈ of generalized external forces τ.sub.ext with a momentum observer based on at least one of the proprioceptive data and the model; generate an estimate {umlaut over ({circumflex over (q)})}(t) of a second derivative of base and robot generalized coordinates {umlaut over (q)}(t), based on {circumflex over (τ)}.sub.∈ and τ.sub.m; estimate a Cartesian acceleration {umlaut over ({circumflex over (x)})}.sub.D of a point D on the kinematic chain structure based on {umlaut over ({circumflex over (q)})}(t); compensate F.sub.S for rigid body dynamics effects based on {umlaut over ({circumflex over (x)})}.sub.D and for gravity effects to obtain an estimated external wrench {circumflex over (F)}.sub.ext,S.i; compensate {circumflex over (τ)}.sub.∈ for a Jacobian J.sub.S.distal.i.sup.T transformed F.sub.ext,S.distal.i to obtain an estimation {circumflex over (τ)}.sub.ext,col of generalized joint forces originating from unexpected collisions; and detect a collision based on given thresholds τ.sub.thresh and F.sub.S.i,thresh if {circumflex over (τ)}.sub.ext,col>τ.sub.thresh and/or if {circumflex over (F)}.sub.ext,S.i>F.sub.S.i,thresh.

8. The robot according to claim 7, wherein the robot comprises a data interface with a data network, and wherein the robot is designed and set up to download system programs for setting up and controlling the robot from the data network.

9. The robot according to claim 7, wherein the robot is designed and set up to download parameters for the system programs from the data network.

10. The robot according to claim 7, wherein the robot is designed and set up to enter parameters for the system programs via a local input interface and/or via a teach-in process, and wherein the robot is manually guided.

11. The robot according to claim 7, wherein the robot is designed and set up such that downloading system programs and/or respective parameters from the data network is controlled by a remote station, the remote station being part of the data network.

12. The robot according to claim 7, wherein the robot is designed and set up such that system programs and/or respective parameters locally available at the robot are sent to one or more participants of the data network based on a respective request received from the data network.

13. The robot according to claim 7, wherein the robot is designed and set up such that system programs with respective parameters available locally at the robot are capable of being started from a remote station, the remote station being part of the data network.

14. The robot according to claim 7, wherein the robot is designed and set up such that the remote station and/or the local input interface comprises a human-machine interface HMI designed and set up for entry of system programs and respective parameters, and/or for selecting system programs and respective parameters from a multitude of system programs and respective parameters.

15. The robot according to claim 14, wherein the human-machine interface HMI is designed and set up such that entries are possible via drag-and-drop entry on a touchscreen, a guided dialogue, a keyboard, a computer-mouse, a haptic interface, a virtual-reality interface, an augmented-reality interface, an acoustic interface, via a body tracking interface, based on electromyographic data, based on electroencephalographic data, via a neuronal interface, or a combination thereof.

16. The robot according to claim 14, wherein the human-machine interface HMI is designed and set up to deliver additive, visual, haptic, olfactory, tactile, electrical feedback, or a combination thereof.

17. The robot according to claim 7, further designed and set up to generate recursively compensation wrenches for compensating the external forces F.sub.S for rigid body dynamics effects based on {umlaut over ({circumflex over (x)})}.sub.D and for gravity effects to obtain an estimated external wrench {circumflex over (F)}.sub.ext,S.i, if multiple sensors S are attached to one of the joints or links.

18. The robot according to claim 7, further designed and set up to determine contact locations r.sub.C of collisions with the kinematic chain structure by calculating a line of force action r.sub.d+λf.sub.i/∥f.sub.i∥ with r.sub.d=(S.sup.T (f.sub.i)).sup.#m.sub.i of the collision and by intersecting the line of force action with the kinematic chain structure geometry.

19. The robot according to claim 18, further designed and set up to: determine full contact Jacobians J.sub.C.i=J.sub.c,iJ.sub.i based on the determined contact locations r.sub.C; and determine the external wrenches (F.sub.ext,1.sup.T . . . F.sub.ext,n.sup.T).sup.T=(J.sub.C.1.sup.T . . . J.sub.C.n.sup.T).sup.#τ.sub.ext,col.

20. The robot according to claim 18, further designed and set up to control the robot dependent on r.sub.C and (F.sub.ext,1.sup.T . . . F.sub.ext,n.sup.T).sup.T.

21. A system for collision handling of a robot, the robot comprising a kinematic chain structure comprising at least one kinematic chain, wherein the kinematic chain structure comprises a base, links, joints connecting the links, actuators and at least one end-effector, a sensor S.sub.distal.i in a most distal link of at least one of the kinematic chains for measuring/estimating force/torque, and sensors S.sub.i for measuring/estimating proprioceptive data, wherein the sensors S.sub.i are arbitrarily positioned along the kinematic chain structure, the system comprising: a data processing device; and a memory storing instructions that, when executed by the data processing device, cause the data processing device to perform operations comprising: providing a model describing dynamics of the robot; measuring and/or estimating with sensor S.sub.distal.i force or torque F.sub.ext,S.distal.i in the most distal link of at least one of the kinematic chains; measuring and/or estimating with the sensors S.sub.i proprioceptive data: base and robot generalized coordinates q(t) and their time derivative {dot over (q)}(t), generalized joint motor forces τ.sub.m, external forces F.sub.S, a base orientation φ.sub.B (t) and a base velocity {dot over (x)}(t).sub.B; generating an estimate {circumflex over (τ)}.sub.∈ of generalized external forces τ.sub.ext with a momentum observer based on at least one of the proprioceptive data and the model; generating an estimate {umlaut over ({circumflex over (q)})}(t) of a second derivative of base and robot generalized coordinates {umlaut over (q)}(t), based on {circumflex over (τ)}.sub.∈ and τ.sub.m; estimating a Cartesian acceleration {umlaut over ({circumflex over (x)})}.sub.D of a point D on the kinematic chain structure based on {umlaut over ({circumflex over (q)})}(t); compensating F.sub.S for rigid body dynamics effects based on {umlaut over ({circumflex over (x)})}.sub.D and for gravity effects to obtain an estimated external wrench {circumflex over (F)}.sub.ext,S.i; compensating {circumflex over (τ)}.sub.∈ for a Jacobian J.sub.S.distal.i.sup.T transformed F.sub.ext,S.distal.i to obtain an estimation {circumflex over (τ)}.sub.ext,col of generalized joint forces originating from unexpected collisions; and detecting a collision based on given thresholds τ.sub.thresh and F.sub.S.i,thresh if {circumflex over (τ)}.sub.ext,col>τ.sub.thresh and/or if {circumflex over (F)}.sub.ext,S.i>F.sub.S.i,thresh.

22. A non-transitory storage medium storing instructions for collision handling of a robot, the robot comprising a kinematic chain structure comprising at least one kinematic chain, wherein the kinematic chain structure comprises a base, links, joints connecting the links, actuators and at least one end-effector, a sensor S.sub.distal.i in a most distal link of at least one of the kinematic chains for measuring/estimating force/torque, and sensors S.sub.i for measuring/estimating proprioceptive data, wherein the sensors S.sub.i are arbitrarily positioned along the kinematic chain structure, the instructions when executed by a data processing device cause the data processing device to perform operations comprising: providing a model describing dynamics of the robot; measuring and/or estimating with sensor S.sub.distal.i force or torque F.sub.ext,S.distal.i in the most distal link of at least one of the kinematic chains; measuring and/or estimating with the sensors S.sub.i proprioceptive data: base and robot generalized coordinates q(t) and their time derivative {dot over (q)}(t), generalized joint motor forces τ.sub.m, external forces F.sub.S, a base orientation φ.sub.B (t) and a base velocity {dot over (x)}(t).sub.B; generating an estimate {circumflex over (τ)}.sub.∈ of generalized external forces τ.sub.ext with a momentum observer based on at least one of the proprioceptive data and the model; generating an estimate {umlaut over ({circumflex over (q)})}(t) of a second derivative of base and robot generalized coordinates {umlaut over (q)}(t), based on {circumflex over (τ)}.sub.∈ and τ.sub.m; estimating a Cartesian acceleration {umlaut over ({circumflex over (x)})}.sub.D of a point D on the kinematic chain structure based on {umlaut over ({circumflex over (q)})}(t); compensating F.sub.S for rigid body dynamics effects based on {umlaut over ({circumflex over (x)})}.sub.D and for gravity effects to obtain an estimated external wrench {circumflex over (F)}.sub.ext,S.i; compensating {circumflex over (τ)}.sub.∈ for a Jacobian J.sub.S.distal.i.sup.T transformed F.sub.ext,S.distal.i to obtain an estimation {circumflex over (τ)}.sub.ext,col of generalized joint forces originating from unexpected collisions; and detecting a collision based on given thresholds τ.sub.thresh and F.sub.S.i,thresh if {circumflex over (τ)}.sub.ext,col>τ.sub.thresh and/or if {circumflex over (F)}.sub.ext,S.i>F.sub.S.i,thresh.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) In the drawings:

(2) FIG. 1 shows a method for collision handling for a robot according to an embodiment of the invention,

(3) FIG. 2 shows a method for collision handling for a robot according to another embodiment of the invention,

(4) FIG. 3 shows a humanoid robot according to another embodiment of the invention,

(5) FIG. 4 shows a detailed excerpt of the method shown in FIG. 1, and

(6) FIG. 5 shows another explanation of the method of FIG. 2.

DETAILED DESCRIPTION

(7) FIG. 1 shows a method of collision handling for a robot 1 with a kinematic chain structure comprising at least one kinematic chain, wherein the kinematic chain structure includes: a base, links, joints connecting the links, actuators and at least one end-effector, a sensor S.sub.distal.i in the most distal link of at least one of the kinematic chains for measuring/estimating force/torque, and sensors S.sub.i for measuring/estimating proprioceptive data, wherein the sensors S.sub.i are arbitrarily positioned along the kinematic chain structure.

(8) The method according to FIG. 1 includes the following steps: providing (S1) a model describing the dynamics of the robot 1; measuring and/or estimating (S2) with sensor S.sub.distal.i force/torque F.sub.ext,S.distal.i in the most distal link of at least one of the kinematic chains; measuring and/or estimating (S3) with the sensors S.sub.i proprioceptive data: base and robot generalized coordinates q(t) and their time derivative {dot over (q)}(t), generalized joint motor forces τ.sub.m, external forces F.sub.S, a base orientation φ.sub.B(t) and a base velocity {dot over (x)}(t).sub.B; generating (S4) an estimate {circumflex over (τ)}.sub.∈ of the generalized external forces τ.sub.ext with a momentum observer 3 based on at least one of the proprioceptive data and the model; generating (S5) an estimate {umlaut over ({circumflex over (q)})}(t) of a second derivative of base and robot generalized coordinates {umlaut over (q)}(t), based on {circumflex over (τ)}.sub.∈ and τ.sub.m; estimating (S6) a Cartesian acceleration {umlaut over ({circumflex over (x)})}.sub.D of a point D on the kinematic chain structure based on {umlaut over ({circumflex over (q)})}(t); compensating (S7) F.sub.S for rigid body dynamics effects based on {umlaut over ({circumflex over (x)})}.sub.D and for gravity effects to obtain an estimated external wrench {circumflex over (F)}.sub.ext,S.i; compensating (S8) {circumflex over (τ)}.sub.∈ for the Jacobian J.sub.S.distal.i.sup.T transformed F.sub.ext,S.distal.i to obtain an estimation {circumflex over (τ)}.sub.ext,col of generalized joint forces originating from unexpected collisions; and detecting (S9) a collision based on given thresholds τ.sub.thresh and F.sub.S.i,thresh if {circumflex over (τ)}.sub.ext,col>τ.sub.thresh and/or if {circumflex over (F)}.sub.ext,S.i>F.sub.S.i,thresh.

(9) FIG. 2 shows a method of collision handling for a robot 1 with a kinematic chain structure comprising at least one kinematic chain, wherein the kinematic chain structure comprises: a base, links, joints connecting the links, actuators and at least one end-effector, a sensor S.sub.distal.i in the most distal link of at least one of the kinematic chains for measuring/estimating force/torque, and sensors S.sub.i for measuring/estimating proprioceptive data, wherein the sensors S.sub.i are arbitrarily positioned along the kinematic chain structure.

(10) The method according to FIG. 2 includes the following steps: providing S1 a model describing the dynamics of the robot 1; measuring and/or estimating (S2) with sensor S.sub.distal.i force/torque F.sub.ext,S.distal.i in the most distal link of at least one of the kinematic chains; measuring and/or estimating S3 with the sensors S.sub.i proprioceptive data: base and robot generalized coordinates q(t) and their time derivative {dot over (q)}(t), generalized joint motor forces τ.sub.m, external forces F.sub.S, a base orientation φ.sub.B(t) and a base velocity {dot over (x)}(t).sub.B; generating (S4) an estimate {circumflex over (τ)}.sub.∈ of the generalized external forces τ.sub.ext with a momentum observer 3 based on at least one of the proprioceptive data and the model; generating (S5) an estimate {umlaut over ({circumflex over (q)})}(t) of a second derivative of base and robot generalized coordinates {umlaut over (q)}(t), based on {circumflex over (τ)}.sub.∈ and τ.sub.m; estimating (S6) a Cartesian acceleration {umlaut over ({circumflex over (x)})}.sub.D of a point D on the kinematic chain structure based on {umlaut over ({circumflex over (q)})}(t); compensating (S7) F.sub.S for rigid body dynamics effects based on {umlaut over ({circumflex over (x)})}.sub.D and for gravity effects to obtain an estimated external wrench {circumflex over (F)}.sub.ext,S.i; compensating (S8) {circumflex over (τ)}.sub.∈ for the Jacobian J.sub.S.distal.i.sup.T transformed F.sub.ext,S.distal.i to obtain an estimation {circumflex over (τ)}.sub.ext,col of generalized joint forces originating from unexpected collisions; detecting (S9) a collision based on given thresholds τ.sub.thresh and F.sub.S.i,thresh if {circumflex over (τ)}.sub.ext,col>τ.sub.thresh and/or if {circumflex over (F)}.sub.ext,S.i>F.sub.S.i,thresh; determining (S10) contact locations r.sub.C of collisions with the kinematic chain structure by calculating a line of force action r.sub.d+λf.sub.i/∥f.sub.i∥ with r.sub.d=(S.sup.T(f.sub.i)).sup.# m.sub.i of the collision and by intersecting the line of force action with the kinematic chain structure geometry; based on the determined contact locations r.sub.C determining (S11) the full contact Jacobians J.sub.C.i=J.sub.c,iJ.sub.i; determining (S12) the external wrenches: (F.sub.ext,1.sup.T . . . F.sub.ext,n.sup.T).sup.T=(J.sub.C.1.sup.T . . . J.sub.C.n.sup.T).sup.#τ.sub.ext,col; and controlling (S13) the robot 1 dependent on r.sub.C and (F.sub.ext,1.sup.T, . . . F.sub.ext,n.sup.T).sup.T.

(11) FIG. 3 shows a humanoid robot 1 with a base B and arms and legs as kinematic chains, wherein a base orientation is denoted as φ.sub.B(t) and a base velocity is denoted as {dot over (x)}(t).sub.B. The humanoid robot 1 is in a multiple contact situation. Generalized external forces F.sub.ext,i are acting all over its structure. Forces on the feet originate from locomotion, forces on the hands originate from manipulation. Other external forces are caused by unwanted collisions. In addition, a number of force/torque sensors S.sub.i (five in the case of FIG. 3) are distributed arbitrarily along the kinematic chain structure of the robot 1. Two contacts are detected at the right arm. One by sensor S.sub.3 and one by sensor S.sub.1, as long as one contact is behind S.sub.1 and the other between S.sub.3 and S. As this is the case for F.sub.ext,2 and F.sub.ext,5 these two wrenches are estimated correctly. (Otherwise, the preferred proceeding is shown with equation (28)).

(12) FIG. 4 shows a detailed excerpt of the method shown in FIG. 1. Exclusion of external wrenches at the distal links F.sub.ext,S.distal.i measured by force/torque sensors S.sub.i from the observed generalized joint forces of equation (4) is done with the upper general description. These wrenches are therefore compensated according to equation (19) from the upper general description and allow for the mapping of F.sub.S.distal.i to F.sub.ext,S.distal.i with the load compenstion symbolized by a rectangle receiving F.sub.S.distal.i and {umlaut over ({circumflex over (q)})},{umlaut over ({circumflex over (q)})} from the observer 3. All other notations and steps are described under the description corresponding to FIG. 1. The upper right dotted block outputting F.sub.S.distal.i is a filter.

(13) FIG. 5 shows a different view of the steps explained under FIG. 2 and shows an overview of the collision detection, isolation, and identification algorithm. Hence, all steps S1 . . . S13 referred to under the description of FIG. 2 can also be applied to FIG. 5. Moreover, the general description from above, in particular, equations (17) to (19) are applicable. The more sensed or estimated or generated information is used, the more information can be obtained from the collision detection. If the collision detection is based on τ.sub.ext,col only, the contact cannot be fully located. If F.sub.ext,S.i is used in addition, the contact can be located to the parts of the robot lying between the detecting and the following sensors. If the full F.sub.ext,S.i are available, collision detection can be done on a per link basis.

LIST OF REFERENCE NUMERALS

(14) 1 robot 3 observer S1 provide S2 measure and/or estimate S3 measure and/or estimate S4 generate S5 generate S6 estimate S7 compensate S8 compensate S9 detect S10 determine S11 determine S12 determine S13 control