Real-time self collision and obstacle avoidance

• US 8,170,287 B2
• Filed: 10/24/2008
• Issued: 05/01/2012
• Est. Priority Date: 10/26/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, Zhu, Youding, Fujimura, Kikuo
• Primary Examiner(s): Bali, Vikkram
• Assistant Examiner(s): Bitar, Nancy
• Application Number:
  US12/257,664
• Publication Number:
  US 20090074252A1
• Time in Patent Office: 1,285 Days
• Field of Search: 382/128, 382/132, 382/134, 382/209, 382/195, 382/180, 382/107, 382/159, 382/106, 382/115, 382/116, 382/117, 382/118, 382/103, 382/285, 382/154, 382/168, 382/181, 382/184, 382/190, 700/245, 700/264, 700/424, 345/473, 345/474
• US Class Current: 382/106
• CPC Class Codes: B25J 9/1666 Avoiding collision or forbidden zones
• CPC Class Codes: G11C 11/5621 using charge storage in a floating gate
• CPC Class Codes: G11C 16/349 Arrangements for evaluating degradation, retention or wearout, e.g. by counting erase cycles

Abstract

A system, method, and computer program product for avoiding collision of a body segment with unconnected structures in an articulated system are described. A virtual surface is constructed surrounding an actual surface of the body segment. Distances between the body segment and unconnected structures are monitored. Responding to an unconnected structure penetrating the virtual surface, a redirected joint motion that prevents the unconnected structure from penetrating deeper into the virtual surface is determined. The body segment is redirected based on the redirected joint motion to avoid colliding with the unconnected structure.

Claims

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

controlling motion of a body segment towards a target based on a generated joint motion;

constructing a virtual surface surrounding an actual surface of the body segment, the virtual surface comprising a plurality of virtual surface points each located at least a nonzero distance away from a corresponding surface point on an actual surface of the body segment in a direction of a unit normal vector from the surface point;

monitoring a distance between the actual surface and an unconnected structure;

detecting that the unconnected structure penetrates the virtual surface;

determining a collision point on the actual surface of the body segment;

determining a redirected joint motion of the collision point that prevents the unconnected structure from penetrating deeper into the virtual surface; and

redirecting the body segment toward the target based on the redirected joint motion without colliding with the unconnected structure.

2. The method of claim 1, wherein the virtual surface is a 3-dimentional virtual surface.

3. The method of claim 1, wherein determining the redirected joint motion further comprises:

determining a redirected motion of the collision point that prevents the unconnected structure from penetrating deeper into the virtual surface; and

determining the redirected joint motion of the collision point based on the redirected motion of the collision point.

4. The method of claim 3, wherein the redirected motion of the collision point is in a direction tangent to the actual surface at the collision point.

5. The method of claim 3, wherein the redirected motion of the collision point is in a direction away from the unconnected structure.

6. The method of claim 1, further comprising:

receiving a motion descriptor of a feature point; and

generating the generated joint motion based on the motion descriptor.

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

8. The method of claim 6, wherein determining the redirected joint motion comprises:

determining a second redirected joint motion that directs the body segment towards the target while preventing the unconnected structure from penetrating deeper into the virtual surface;

determining a redesigned motion descriptor of the feature point based on the second redirected joint motion; and

determining the redirected joint motion based on the redesigned motion descriptor.

9. The method of claim 1, wherein determining the redirected joint motion comprises determining the redirected joint motion using a blending function to preserve a motion continuity of the body segment during the redirection.

10. The method of claim 9, wherein the blending function is a Sigmoidal function.

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

12. The method of claim 1, wherein the unconnected structure comprises a second body segment in the articulated system, and wherein the method further comprises:

determining a second redirected joint motion for the second body segment that prevents the second body segment from colliding with the body segment; and

redirecting the second body segment based on the second redirected joint motion to avoid colliding with the body segment.

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

controlling motion of a body segment towards a target based on a generated joint motion;

constructing a virtual surface surrounding an actual surface of the body segment, the virtual surface comprising a plurality of virtual surface points each located at least a nonzero distance away from a corresponding surface point on an actual surface of the body segment in a direction of a unit normal vector from the surface point;

monitoring a distance between the actual surface and an unconnected structure;

detecting that the unconnected structure penetrates the virtual surface;

determining a collision point on the actual surface of the body segment;

determining a redirected joint motion of the collision point that prevents the unconnected structure from penetrating deeper into the virtual surface; and

redirecting the body segment toward the target based on the redirected joint motion without colliding with the unconnected structure.

14. The computer program product of claim 13, wherein the virtual surface is a 3-dimentional virtual surface.

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

determining a redirected motion of the collision point that prevents the unconnected structure from penetrating deeper into the virtual surface; and

determining the redirected joint motion of the collision point based on the redirected motion of the collision point.

16. The computer program product of claim 15, wherein the redirected motion of the collision point is in a direction tangent to the actual surface at the collision point.

17. The computer program product of claim 15, wherein the redirected motion of the collision point is in a direction away from the unconnected structure.

18. The computer program product of claim 13, wherein the method further comprises:

receiving a motion descriptor of a feature point; and

generating the generated joint motion based on the motion descriptor.

19. The computer program product of claim 18, wherein the motion descriptor describes a human motion, the articulated system comprises a humanoid robot, and the joint motion controls the humanoid robot to simulate the human motion.

20. The computer program product of claim 18, wherein determining the redirected joint motion comprises:

determining a second redirected joint motion that directs the body segment towards the target while preventing the unconnected structure from penetrating deeper into the virtual surface;

determining a redesigned motion descriptor of the feature point based on the second redirected joint motion; and

determining the redirected joint motion based on the redesigned motion descriptor.

21. The computer program product of claim 13, wherein determining the redirected joint motion comprises determining the redirected joint motion using a blending function to preserve a motion continuity of the body segment during the redirection.

22. The computer program product of claim 21, wherein the blending function is a Sigmoidal function.

23. The computer program product of claim 13, wherein the redirected joint motion is determined using a closed loop inverse kinematics (CLIK) algorithm.

24. The computer program product of claim 13, wherein the unconnected structure comprises a second body segment in the articulated system, and wherein the method further comprises:

determining a second redirected joint motion for the second body segment that prevents the second body segment from colliding with the body segment; and

redirecting the second body segment based on the second redirected joint motion to avoid colliding with the body segment.

25. 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;

controlling motion of a body segment towards a target based on a generated joint motion;

constructing a virtual surface surrounding an actual surface of the body segment, the virtual surface comprising a plurality of virtual surface points each located at least a nonzero distance away from a corresponding surface point on an actual surface of the body segment in a direction of a unit normal vector from the surface point;

monitoring a distance between the actual surface and an unconnected structure;

detecting that the unconnected structure penetrates the virtual surface;

determining a collision point on the actual surface of the body segment;

determining a redirected joint motion of the collision point that prevents the unconnected structure from penetrating deeper into the virtual surface; and

redirecting the body segment toward the target based on the redirected joint motion without colliding with the unconnected structure.

26. The system of claim 25, wherein the virtual surface is a 3-dimentional virtual surface.

27. The system of claim 25, wherein the method further comprises:

determining a redirected motion of the collision point that prevents the unconnected structure from penetrating deeper into the virtual surface; and

determining the redirected joint motion of the collision point based on the redirected motion of the collision point.

28. The system of claim 27, wherein the redirected motion of the collision point is in a direction tangent to the actual surface at the collision point.

29. The system of claim 27, wherein the redirected motion of the collision point is in a direction away from the unconnected structure.

30. The system of claim 25, wherein the method further comprises:

receiving a motion descriptor of a feature point; and

generating the generated joint motion based on the motion descriptor.

31. The system of claim 30, wherein the motion descriptor describes a human motion, the articulated system comprises a humanoid robot, and the joint motion controls the humanoid robot to simulate the human motion.

32. The system of claim 30, wherein determining the redirected joint motion comprises:

determining a second redirected joint motion that directs the body segment towards the target while preventing the unconnected structure from penetrating deeper into the virtual surface;

determining a redesigned motion descriptor of the feature point based on the second redirected joint motion; and

determining the redirected joint motion based on the redesigned motion descriptor.

33. The system of claim 25, wherein determining the redirected joint motion comprises determining the redirected joint motion using a blending function to preserve a motion continuity of the body segment during the redirection.

34. The system of claim 33, wherein the blending function is a Sigmoidal function.

35. The system of claim 25, wherein the redirected joint motion is determined using a closed loop inverse kinematics (CLIK) algorithm.

36. The system of claim 25, wherein the unconnected structure comprises a second body segment in the articulated system, and wherein the method further comprises:

determining a second redirected joint motion for the second body segment that prevents the second body segment from colliding with the body segment; and

redirecting the second body segment based on the second redirected joint motion to avoid colliding with the body segment.