Real-time self collision and obstacle avoidance using weighting matrix

• US 8,396,595 B2
• Filed: 10/24/2008
• Issued: 03/12/2013
• Est. Priority Date: 11/01/2007
• Status: Active Grant

• Current Assignee: Honda Motor Co., Ltd. (Honda Motor Company)
• Original Assignee: Honda Motor Co., Ltd. (Honda Motor Company)
• Inventors: Dariush, Behzad
• Primary Examiner(s): Tran, Dalena
• Assistant Examiner(s): OH, HARRY Y
• Application Number:
  US12/258,184
• Publication Number:
  US 20090118863A1
• Time in Patent Office: 1,600 Days
• Field of Search: 700/255, 382106-107, 382/181, 382/259
• US Class Current: 700/255
• CPC Class Codes: B25J 9/1666 Avoiding collision or forbidden zones
• CPC Class Codes: G05B 2219/39096 Self-collision, internal collison, collision between links of one robot
• CPC Class Codes: G05B 2219/40463 Shortest distance in time, or metric, time optimal
• CPC Class Codes: G05B 2219/40497 Collision monitor controls planner in real time to replan if collision
• CPC Class Codes: G06N 3/004 Artificial life, i.e. computing arrangements simulating life

Abstract

A system, method, and computer program product for avoiding collision of a body segment with other structures in an articulated system are described. According to one aspect, a collision function is determined for avoiding such collision. A distance between the body segment and one such structure is measured. A weighting matrix is generated based on the collision function and the distance, and used to determine a redirected motion for the body segment. The body segment is redirected based on the redirected motion to avoid colliding with the structure.

Claims

1. A method for avoiding collision of a body segment in an articulated system, the method comprising:

determining a collision function for avoiding collision between the body segment and other structures in the articulated system;

measuring a distance between the body segment and a first structure, the first structure being one of the other structures;

determining a joint limit function for avoiding self collision between the body segment and the other structures in the articulated system;

measuring a joint vector between the body segment and a second structure, the second structure being one of the other structures;

generating, using a processor, a collision gradient weighting matrix based on the collision function and the measured distance;

generating, using a processor, a joint limit gradient weighting matrix based on the joint limit function and the measured joint vector;

generating a combined weighting matrix based on the collision gradient weighting matrix and the joint limit gradient weighting matrix;

determining a redirected motion that prevents the body segment from colliding with the first structure based on the combined weighting matrix; and

redirecting the body segment based on the redirected motion to avoid colliding with the first structure.

2. The method of claim 1, wherein the first structure is not directly connected with the body segment.

3. The method of claim 1, wherein the collision function comprises elements each representing a degree to which a corresponding degree of freedom influences the distance.

4. The method of claim 3, wherein the collision gradient weighting matrix comprises an n×

n diagonal matrix with diagonal elements, the diagonal elements comprising elements of the collision function.

5. The method of claim 1, wherein the collision function has a maximum value when the distance equals zero and decays exponentially towards zero as the distance increases, and wherein the collision function has an adjustable parameter that affects a rate of decay.

6. The method of claim 1, wherein a value of the collision function varies depending upon the distance between the body segment and the first structure.

7. The method of claim 1, further comprising:

receiving a motion descriptor;

generating a joint variable based on the motion descriptor; and

controlling motion of the body segment toward a target based on the joint variable,

wherein determining the redirected motion comprises determining the redirected motion that directs the body segment towards the target while preventing the body segment from colliding with the first structure, and

wherein redirecting the body segment further comprises redirecting the body segment toward the target without colliding with the first structure.

8. The method of claim 7, wherein the motion descriptor describes a human motion, the articulated system comprises a humanoid robot, and the joint variable controls the humanoid robot to simulate the human motion.

9. The method of claim 1, wherein redirecting the body segment further comprises:

generating a redirected motion descriptor based on the redirected motion;

generating a redirected joint variable based on the redirected motion descriptor; and

redirecting the body segment based on the redirected joint variable to avoid colliding with the first structure.

10. The method of claim 1, wherein the combined weighting matrix is normalized by a total number of collision pairs in the articulated system.

11. The method of claim 1, wherein the redirected motion is calculated using a closed loop inverse kinematics (CLIK) algorithm.

12. A computer program product for avoiding collision of a body segment in an articulated system, the computer program product configured to be stored in a non-transitory computer-readable storage medium, the computer program product comprising executable computer program code for performing a method comprising:

determining a collision function for avoiding collision between the body segment and other structures in the articulated system;

measuring a distance between the body segment and a first structure, the first structure being one of the other structures;

determining a joint limit function for avoiding self collision between the body segment and the other structures in the articulated system;

measuring a joint vector between the body segment and a second structure, the second structure being one of the other structures;

generating, using a processor, a collision gradient weighting matrix based on the collision function and the measured distance;

generating, using a processor, a joint limit gradient weighting matrix based on the joint limit function and the measured joint vector;

generating a combined weighting matrix based on the collision gradient weighting matrix and the joint limit gradient weighting matrix;

determining a redirected motion that prevents the body segment from colliding with the first structure based on the combined weighting matrix; and

redirecting the body segment based on the redirected motion to avoid colliding with the first structure.

13. The computer program product of claim 12, wherein the collision function comprises elements each representing a degree to which a corresponding degree of freedom influences the distance.

14. The computer program product of claim 13, wherein the collision gradient weighting matrix comprises an n×

n diagonal matrix with diagonal elements, the diagonal elements comprising elements of the collision function.

15. The computer program product of claim 12, wherein the method further comprises:

receiving a motion descriptor;

generating a joint variable based on the motion descriptor; and

controlling motion of the body segment toward a target based on the joint variable,

wherein determining the redirected motion comprises determining the redirected motion that directs the body segment towards the target while preventing the body segment from colliding with the first structure, and

wherein redirecting the body segment further comprises redirecting the body segment toward the target without colliding with the first structure.

16. A system for avoiding collision of a body segment in an articulated system, the system comprising:

a computer processor for executing executable computer program code;

a computer-readable storage medium containing the executable computer program code for performing a method comprising;

determining a collision function for avoiding collision between the body segment and other structures in the articulated system;

measuring a distance between the body segment and a first structure, the first structure being one of the other structures;

determining a joint limit function for avoiding self collision between the body segment and the other structures in the articulated system;

measuring a joint vector between the body segment and a second structure, the second structure being one of the other structures;

generating, using a processor, a collision gradient weighting matrix based on the collision function and the measured distance;

generating, using a processor, a joint limit gradient weighting matrix based on the joint limit function and the measured joint vector;

generating a combined weighting matrix based on the collision gradient weighting matrix and the joint limit gradient weighting matrix;

determining a redirected motion that prevents the body segment from colliding with the first structure based on the combined weighting matrix; and

redirecting the body segment based on the redirected motion to avoid colliding with the first structure.

17. The system of claim 16, wherein the collision function comprises elements each representing a degree to which a corresponding degree of freedom influences the distance.

18. The system of claim 17, wherein the collision gradient weighting matrix comprises an n×

n diagonal matrix with diagonal elements, the diagonal elements comprising elements of the collision function.

19. The system of claim 16, wherein the method further comprises:

receiving a motion descriptor;

generating a joint variable based on the motion descriptor; and

controlling motion of the body segment toward a target based on the joint variable,

wherein determining the redirected motion comprises determining the redirected motion that directs the body segment towards the target while preventing the body segment from colliding with the first structure, and

wherein redirecting the body segment further comprises redirecting the body segment toward the target without colliding with the first structure.