Method and apparatus of free space enumeration for collision avoidance

• US 5,047,916 A
• Filed: 03/24/1989
• Issued: 09/10/1991
• Est. Priority Date: 03/25/1988
• Status: Expired due to Fees

• Current Assignee: Toshiba Corporation
• Original Assignee: Kabushiki Kaisha Toshiba (Toshiba Corporation)
• Inventors: Kondo, Koichi
• Primary Examiner(s): RUGGIERO, JOSEPH F
• Application Number:
  US07/328,682
• Time in Patent Office: 900 Days
• Field of Search: 364/513, 364/461, 364/474.2, 364/167.01, 901/1, 901/50, 318/568.12, 318/587
• US Class Current: 700/61
• CPC Class Codes: B25J 9/1666 Avoiding collision or forbidden zones

Abstract

A method and an apparatus for free space enumeration for motion planning, having general applicability while reducing amounts of information and calculations involved, capable of modifying a path, capable of setting up appropriate configuration space quantization, and capable of adopting appropriate strategy for free space enumeration. The method may include the step of selecting cells only between the initial point and the final point, or selecting cells between the initial point and the final point, using a plurality of strategies for selecting the cells simultaneously, or dividing the configuration space into multiplicity of cells defined in terms of intervals in the degrees of freedom, or determining a collision-free path in the free space joining the initial point and the final point without collision, or modifying the collision-free path.

Claims

1. An apparatus for collision-free path determination using free space enumeration, in which configurations of a physical object and surrounding physical obstacles are represented by a configuration space defined in terms of degrees of freedom of the object, the configuration space being divided up into a multiplicity of cells defined in terms of intervals in the degrees of freedom, in which a path of the object between a given initial point in the configuration space from which the path starts and a given final point in the configuration space at which the path ends is to be determined, the apparatus comprising:

means for enumerating a continuous free space between the initial point and the final point in which the path can be located, including;

means for selecting cells which satisfy a prescribed condition, between the initial point and the final point, by simultaneously using a plurality of strategies for selecting the cells; and

means for checking an occurrence of a collision between the object and the surrounding obstacles in the selected cells;

means for determining a collision-free path in the free space joining the initial point and the final point; and

means for controlling a position of the physical object with respect to the physical obstacle according to the determined collision-free path.

2. The apparatus of claim 1, wherein all the selections by the plurality of strategies discontinues when the cells selected by one of the strategies succeed to connect the initial point and the final point continuously.

3. The apparatus of claim 1, wherein the selecting means further including:

means for inspecting progress of the selection by each of the plurality of strategies at intermediate points between the initial point and the final point; and

means for deterring the selections by those strategies found by the inspections to be slower in progress than at least one of the other strategies.

4. The apparatus of claim 3, wherein the inspecting means utilizes a quantity P defined for a point in the configuration space by:

space="preserve" listing-type="equation">P=C.sup.N /F

where C is a number of cells from the initial point, N is a dimensionality of the configuration space, and F is a total number of cells in the free space already enumerated.

5. The apparatus of claim 1, wherein the selecting means selects cells according to a heuristic function defined for each cell as f=c+h where c is a number of cells from the initial point and h is an estimated number of cells to the final point such that the cell for which the heuristic function takes a minimum value and wherein the different strategies being given by the estimated number of cells h are different.

6. The apparatus of claim 5, wherein the estimated number of cells h at a particular cell is obtained as:

##EQU4## where G is an overall coefficient, each a(i) is a coefficient for each degree of freedom of the object, c(i) is a value of i-th degree of freedom at the particular cell, and Fc(i) is a value of i-th degree of freedom at the final point, and wherein the different strategies being given by at least one of the overall coefficient G and coefficients a(i) different from each other.

7. The apparatus of claim 6, wherein at least one of the overall coefficient G and coefficients a(i) are determined within a prescribed range of values given for each.

8. The apparatus of claim 7, wherein those of the overall coefficient G and coefficients a(i) which are to be determined within a prescribed range of values given for each, are determined as a random number within the range.

9. The apparatus of claim 7, wherein the range is narrower for a coefficient which has a smaller difference between the initial point and the final point.

10. An apparatus for collision-free path determination using free space enumeration, in which configurations of a physical object and surrounding physical obstacles are represented by a configuration space defined in terms of degrees of freedom of the object, the configuration space being divided up into a multiplicity of cells defined in terms of intervals in the degrees of freedom, in which a path of the object between a given initial point in the configuration space from which the path starts and a given final point in the configuration space at which the path ends is to be determined, the apparatus comprising:

means for enumerating a continuous free space between the initial point and the final point in which the path can be located, including;

means for dividing the configuration space into multiplicity of cells defined in terms of intervals in the degrees of freedom, including;

means for partitioning a physical space into a multiplicity of sub-regions;

means for calculating a change in a total volume of the sub-regions which contains at least a portion of the object when each one of the degrees of freedom is varied separately by one provisionally adapted unit of intervals; and

means for determining the intervals for the degrees of freedom defining the cells in the configuration space in accordance with the calculated change;

means for selecting cells which satisfy a prescribed condition, between the initial point and the final point; and

means for checking an occurrence of a collision between the object and the surrounding obstacles in the selected cells;

means for determining a collision-free path in the free space joining the initial point and the final point without the collision; and

means for controlling a position of the physical object with respect to the physical obstacle according to the determined collision-free path.

11. The apparatus of claim 10, wherein all the sub-regions are identical in shape and volume.

12. The apparatus of claim 10, wherein the partitioning means uses a quad-tree when the physical space is considered to be 2-dimensional, and an oct-tree when the physical space is considered to be 3-dimensional.

13. The apparatus of claim 10, wherein the determining means determines the intervals on a basis of comparison between the calculated change and a prescribed reference value.

14. The apparatus of claim 13, wherein the detemining means determines the intervals such that, when the calculated change is greater than the prescribed reference value, the provisionally adapted unit of the intervals is reduced in proportion to a ratio of the prescribed reference value with respect to the calculated change.

15. An apparatus for collision-free path determination using free space enumeration, in which configurations of a physical object and surrounding physical obstacles are represented by a configuration space defined in terms of degrees of freedom of the object, the configuration space being divided up into a multiplicity of cells defined in terms of intervals in the degrees of freedom, in which a path of the object between a given initial point in the configuration space from which the motion starts and a given final point in the configuration space at which the path ends is to be determined, the apparatus comprising:

means for enumerating a continuous free space between the initial point and the final point in which the path can be located, including;

means for selecting cells which satisfy a prescribed condition, only between the initial point and the final point;

means for checking an occurrence of a collision between the object and the surrounding obstacles in the selected cells; and

means for memorizing positions in the configuration space of free space cells which are the selected cells for which the collision cannot be detected;

means for determining a collision-free path in the free space joining the initial point and the final point without the collision; and

means for controlling a position of the physical object with respect to the physical obstacle according to the determined collision-free path.

16. The apparatus of claim 15, wherein the memorizing means also memorizes positions in the configuration space of collision cells which are the selected cells for which the collision can be detected.

17. The apparatus of claim 15, wherein the configuration space is partitioned into blocks, each block containing a plurality of the cells, and wherein the memorizing means memorizes the positions in the configuration space of the blocks containing at least one of the free space cells, and positions in the blocks of the free space cells.

18. The apparatus of claim 17, wherein the memorizing means also memorizes positions in the configuration space of the blocks containing at least one of collision cells, where the collision cells are the selected cells for which the collision can be detected, and positions in the blocks of the collision cells.

19. The apparatus of claim 15, wherein the selecting means utilizes cell expansions.

20. The apparatus of claim 15, wherein the selecting means selects cells according to a heuristic function defined for each cell as f=c+h, where c is a number of cells from the initial point and h is an estimated number of cells to the final point, such that the cell for which the heuristic function takes a minimum value is selected in succession.

21. The apparatus of claim 20, wherein the estimated number of cells h at a particular cell is obtained as:

##EQU5## where G is an overall coefficient, each a(i) is a coefficient for each degree of freedom of the object, c(i) is a value of i-th degree of freedom at the particular cell, and Fc(i) is a value of i-th degree of freedom at the final point.

22. The apparatus of claim 21, wherein at least one of the overall coefficient G and coefficients a(i) are determined within a prescribed range of values given for each.

23. The apparatus of claim 22, wherein those of the overall coefficient G and coefficients a(i) which are to be determined within a prescribed range of values given for each, are determined as a random number within the range.

24. The apparatus of claim 22, wherein the range is narrower for a coefficient which has a smaller difference between the initial point and the final point.

25. An apparatus for collision-free path determination using free space enumeration, in which configurations of a physical object and surrounding physical obstacles are represented by a configuration space defined in terms of their coordinates, in which a path of the object between a given initial point in the configuration space and a given final point in the configuration space is to be determined, the apparatus comprising:

means for enumerating a continuous free space between the initial point and the final point in which the collision between the object and the surrounding obstacles is not possible;

means for determining a collision-free path in the free space joining the initial point and the final point without the collision;

means for modifying the collision-free path; and

means for controlling a position of the physical object with respect to the physical obstacle according to the modified collision-free path.

26. The apparatus of claim 25, wherein the configuration space is divided up into a multiplicity of cells defined in terms of intervals in the coordinates, and wherein the enumerating means includes:

means for selecting cells only between the initial point and the final point;

means for detecting a collision between the object and the surrounding obstacles in the selected cells; and

means for memorizing positions in the configuration space of free space cells which are the selected cells for which the collision cannot be detected.

27. The apparatus of claim 26, wherein the memorizing means also memorizes positions in the configuration space of collision cells which are the selected cells for which the collision can be detected.

28. The apparatus of claim 25, wherein the modifying means includes:

means for designating a part of the free space as a dangerous part; and

means for adjusting the collision-free path such that the dangerous part is avoided by the collision-free path.

29. The apparatus of claim 28, wherein the deciding means utilizes wavefront expansions from the cells not in the free space.

30. The apparatus of claim 28, wherein the modifying means further includes means for expanding the free space further.

31. The apparatus of claim 30, wherein the expanding means utilizes wavefront expansion.

32. The apparatus of claim 31, wherein the expanding means utilizes a scanning window for the wavefront expansion.

33. An apparatus for collision-free area determination using free space enumeration, in which configurations of a physical object and surrounding physical obstacles are represented by a configuration space defined in terms of their coordinates, in which a location of the object between a given initial point in the configuration space and a given final point in the configuration space is to be determined, the apparatus comprising:

means for enumerating a continuous free space between the initial point and the final point in which the object can be located;

means for determining a collision-free area in the free space joining the initial point and the final point without the collision;

means for modifying the collision-free area; and

means for controlling a position of the physical object with respect to the physical obstacle according to the modified collision-free path.

34. The apparatus of claim 33, wherein the configuration space is divided up into a multiplicity of cells defined in terms of intervals in the coordinates, and wherein the enumerating means includes:

means for selecting cells only between the initial point and the final point;

means for detecting a collision between the object and the surrounding obstacles in the selected cells; and

means for memorizing positions in the configuration space of free space cells which are the selected cells for which the collision cannot be detected.

35. The apparatus of claim 34, wherein the memorizing means also memorizes positions in the configuration space of collision cells which are the selected cells for which the collision can be detected.

36. The apparatus of claim 33, wherein the modifying means includes:

means for designating a part of the free space as a dangerous part; and

means for adjusting the collision-free area such that the dangerous part is avoided by the collision-free area.

37. The apparatus of claim 36, wherein the deciding means utilizes wavefront expansions from the cells not in the free space.

38. The apparatus of claim 36, wherein the modifying means further includes means for expanding the free space further.

39. The apparatus of claim 38, wherein the expanding means utilizes wavefront expansion.

40. The apparatus of claim 39, wherein the expanding means utilizes a scanning window for the wavefront expansion.

41. A method of collision-free path determination using free space enumeration, in which configurations of a physical object and surrounding physical obstacles are represented by a configuration space defined in terms of degrees of freedom of the object, the configuration space being divided up into a multiplicity of cells defined in terms of intervals in the degrees of freedom, in which a path of the object between a given initial point in the configuration space from which the path starts and a given final point in the configuration space at which the path ends is to be determined, the method comprising the steps of:

enumerating a continuous free space between the initial point and the final point in which the path can be located, including the steps of;

selecting cells which satisfy a prescribed condition, between the initial point and the final point, by simultaneously using a plurality of strategies for selecting the cells; and

checking an occurrence of a collision between the object and the surrounding obstacles in the selected cells;

determining a collision-free path in the free space joining the initial point and the final point without the collision; and

controlling a position of the physical object with respect to the physical obstacle according to the determined collision-free path.

42. The method of claim 41, wherein all the selections by the plurality of strategies discontinues when the cells selected by one of the strategies succeed to connect the initial point and the final point continuously.

43. The method of claim 41, wherein the selecting step further including the steps of:

inspecting progress of the selection by each of the plurality of strategies at intermediate points between the initial point and the final point; and

deterring the selections by those strategies found by the inspections to be slower in progress than at least one of the other strategies.

44. The method of claim 43, wherein at the inspecting step a quantity P defined for a point in the configuration space by:

space="preserve" listing-type="equation">P=C.sup.N /F

where C is a number of cells from the initial point, N is a dimensionality of the configuration space, and F is a total number of cells in the free space already enumerated, is utilized.

45. The method of claim 41, wherein at the selecting step the cells are selected according to a heuristic function defined for each cell as f=c+h, where c is a number of cells from the initial point and h is an estimated number of cells to the final point, such that the cell for which the heuristic function takes a minimum value is selected in succession, and wherein the different strategies being given by the estimated number of cells h estimated differently.

46. The method of claim 45, wherein the estimated number of cells h at a particular cell is obtained as:

##EQU6## where G is an overall coefficient, each a(i) is a coefficient for each degree of freedom of the object, c(i) is a value of i-th degree of freedom at the particular cell, and Fc(i) is a value of i-th degree of freedom at the final point, and wherein the different strategies being given by at least one of the overall coefficient G and coefficients a(i) different from each other.

47. The method of claim 46, wherein at least one of the overall coefficient G and coefficients a(i) are determined within a prescribed range of values given for each.

48. The method of claim 47, wherein those of the overall coefficient G and coefficients a(i) which are to be determined within a prescribed range of values given for each, are determined as a random number within the range.

49. The method of claim 47, wherein the range is narrower for a coefficient which has a smaller difference between the initial point and the final point.

50. A method of collision-free path determination using free space enumeration, in which configurations of a physical object and surrounding physical obstacles are represented by a configuration space defined in terms of degrees of freedom of the object, the configuration space being divided up into a multiplicity of cells defined in terms of intervals in the degrees of freedom, in which a path of the object between a given initial point in the configuration space from which the path starts and a given final point in the configuration space at which the path ends is to be determined, the method comprising the steps of:

enumerating a continuous free space between the initial point and the final point in which the path can be located, including the steps of;

dividing the configuration space into multiplicity of cells defined in terms of intervals in the degrees of freedom, including the steps of;

partitioning a physical space into multiplicity of sub-regions;

calculating a change in a total volume of the sub-regions which contains at least a portion of the object when each one of the degrees of freedom is varied separately by one provisionally adapted unit of intervals; and

determining the intervals for the degrees of freedom defining the cells in the configuration space in accordance with the calculated change;

selecting cells which satisfy a prescribed condition, between the initial point and the final point; and

checking an occurrence of a collision between the object and the surrounding obstacles in the selected cells;

determining a collision-free path in the free space joining the initial point and the final point without the collision; and

controlling a position of the physical object with respect to the physical obstacle according to the determined collision-free path.

51. The method of claim 50, wherein all the sub-regions are identical in shape and volume.

52. The method of claim 50, wherein at the partitioning step a use is made of a quad-tree when the physical space is considered to be 2-dimensional, and an oct-tree when the physical space is considered to be 3-dimensional.

53. The method of claim 50, wherein at the determining step the intervals are detemined on a basis of comparison between the calculated change and a prescribed reference value.

54. The method of claim 53, wherein at the detemining step the intervals are determined such that, when the calculated change is greater than the prescribed reference value, the provisionally adapted unit of the intervals is reduced in proportion to a ratio of the prescribed reference value with respect to the calculated change.

55. A method of collision-free path determination using free space enumeration, in which configurations of a physical object and surrounding physical obstacles are represented by a configuration space defined in terms of degrees of freedom of the object, the configuration space being divided up into a multiplicity of cells defined in terms of intervals in the degrees of freedom, in which a path of the object between a given initial point in the configuration space from which the path starts and a given final point in the configuration space at which the path ends is to be determined, the method comprising the steps of:

enumerating a continuous free space between the initial point and the final point in which the path can be located, including the steps of;

selecting cells which satisfy a prescribed condition, only between the initial point and the final point;

checking an occurrence of a collision between the object and the surrounding obstacles in the selected cells; and

memorizing positions in the configuration space of free space cells which are the selected cells for which the collision cannot be detected;

determining a collision-free path in the free space joining the initial point and the final point without the collision; and

controlling a position of the physical object with respect to the physical obstacle according to the determined collision-free path.

56. The method of claim 55, wherein at the memorizing step positions in the configuration space of collision cells which are the selected cells for which the collision can be detected are also memorized.

57. The method of claim 55, wherein the configuration space is partitioned into blocks, each block containing plurality of the cells, and wherein at the memorizing step the positions in the configuration space of the blocks containing at least one of the free space cells, and positions in the blocks of the free space cells are memorized.

58. The method of claim 57, wherein at the memorizing step positions in the configuration space of the blocks containing at least one of collision cells which are the selected cells for which the collision can be detected, and positions in the blocks of the collision cells are also memorized.

59. The method of claim 55, wherein at the selecting step cell expansions are utilized.

60. The method of claim 55, wherein at the selecting step the cells are selected according to a heuristic function defined for each cell as f=c+h, where c is a number of cells from the initial point and h is an estimated number of cells to the final point, such that the cell for which the heuristic function takes a minimum value is selected in succession.

61. The method of claim 60, wherein the estimated number of cells h at a particular cell is obtained as:

##EQU7## where G is an overall coefficient, each a(i) is a coefficient for each degree of freedom of the object, c(i) is a value of i-th degree of freedom at the particular cell, and Fc(i) is a value of i-th degree of freedom at the final point.

62. The method of claim 61, wherein at least one of the overall coefficient G and coefficients a(i) are determined within a prescribed range of values given for each.

63. The method of claim 62, wherein those of the overall coefficient G and coefficients a(i) which are to be determined within a prescribed range of values given for each, are determined as a random number within the range.

64. The method of claim 62, wherein the range is narrower for a coefficient which has a smaller difference between the initial point and the final point.

65. A method of collision-free path determination using free space enumeration, in which configurations of a physical object and surrounding physical obstacles are represented by a configuration space defined in terms of degrees of freedom of the object, the configuration space being divided up into a multiplicity of cells defined in terms of intervals in the degrees of freedom, in which a path of the object between a given initial point in the configuration space from which the path starts and a given final point in the configuration space at which the path ends is to be planned, the method comprising the steps of:

enumerating a continuous free space between the initial point and the final point in which the motion is possible;

determining a collision-free path in the free space joining the initial point and the final point without the collision;

modifying the collision-free path; and

controlling a position of the physical object with respect to the physical obstacle according to the modified collision-free path.

66. The method of claim 65, wherein the enumerating step includes the steps of:

selecting cells only between the initial point and the final point;

checking an occurrence of a collision between the object and the surrounding obstacles in the selected cells; and

memorizing positions in the configuration space of free space cells which are the selected cells for which the collision cannot be detected.

67. The method of claim 66, wherein at the memorizing step positions in the configuration space of collision cells which are the selected cells for which the collision can be detected are also memorized.

68. The method of claim 65, wherein the modifying step includes the steps of:

designating a part of the free space as a dangerous part; and

adjusting the collision-free path such that the dangerous part is avoided by the collision-free path.

69. The method of claim 68, wherein at the deciding step wavefront expansion from the cells not in the free space is utilized.

70. The method of claim 68, wherein the modifying step further includes the step of expanding the free space further.

71. The method of claim 70, wherein at the expanding step wavefront expansion is utilized.

72. The method of claim 71, wherein at the expanding step a scanning window is utilized for the wavefront expansion.

73. A method of collision-free area determination using free space enumeration, in which configurations of a physical object and surrounding physical obstacles are represented by a configuration space defined in terms of their coordinates, and in which a location of the object between a given initial point in the configuration space and a given final point in the configuration space is to be determined, the method comprising the steps of:

enumerating a continuous free space between the initial point and the final point in which the object can be located;

determining a collision-free area in the free space joining the initial point and the final point without the collision;

modifying the collision-free area; and

controlling a position of the physical object with respect to the physical obstacle according to the modified collision-free area.

74. The method of claim 73, wherein the configuration space being divided up into multiplicity of cells defined in terms of intervals in the coordinates, and wherein the enumerating step includes the steps of:

selecting cells only between the initial point and the final point;

checking an occurrence of a collision between the object and the surrounding obstacles in the selected cells; and

memorizing positions in the configuration space of free space cells which are the selected cells for which the collision cannot be detected.

75. The method of claim 74, wherein at the memorizing step positions in the configuration space of collision cells which are the selected cells for which the collision can be detected are also memorized.

76. The method of claim 73, wherein the modifying step includes the steps of:

designating a part of the free space as a dangerous part; and

adjusting the collision-free area such that the dangerous part is avoided by the collision-free area.

77. The method of claim 76, wherein at the deciding step wavefront expansions from the cells not in the free space is utilized.

78. The method of claim 76, wherein the modifying step further includes the step of expanding the free space further.

79. The method of claim 78, wherein at the expanding means wavefront expansion is utilized.

80. The method of claim 79, wherein at the expanding means a scanning window for the wavefront expansion is utilized.