Method and apparatus for path planning

• US 6,604,005 B1
• Filed: 11/16/1990
• Issued: 08/05/2003
• Est. Priority Date: 11/20/1987
• Status: Expired due to Term

• Current Assignee: Koninklijke Philips Electronics N.V. (Koninklijke Philips N.V.)
• Original Assignee: Koninklijke Philips Electronics N.V. (Koninklijke Philips N.V.)
• Inventors: Trovato, Karen Irene, Dorst, Leendert
• Primary Examiner(s): PATEL, RAMESH B
• Application Number:
  US07/617,303
• Time in Patent Office: 4,645 Days
• Field of Search: 901/1, 901/50, 700/28, 700/56, 700/245, 700/250, 700/253, 700/254, 700/255, 701/23, 701/25, 701/26, 701/117, 701/118, 701/201-202, 701/210, 706/905, 706/919
• US Class Current: 700/56
• CPC Class Codes: B25J 9/1666 Avoiding collision or forbidden zones
• CPC Class Codes: G05B 19/4061 Avoiding collision or forbidden zones
• CPC Class Codes: G05B 2219/35415 3-D three dimension, space input, spaceball
• CPC Class Codes: G05B 2219/39217 Keep constant orientation of handled object while moving manipulator
• CPC Class Codes: G05B 2219/40443 Conditional and iterative planning
• CPC Class Codes: G05B 2219/40448 Preprocess nodes with arm configurations, c-space and planning by connecting nodes
• CPC Class Codes: G05B 2219/40465 Criteria is lowest cost function, minimum work path
• CPC Class Codes: G05B 2219/40471 Using gradient method
• CPC Class Codes: G05B 2219/40476 Collision, planning for collision free path
• CPC Class Codes: G05B 2219/45131 Turret punch press
• CPC Class Codes: G05B 2219/49143 Obstacle, collision avoiding control, move so that no collision occurs
• CPC Class Codes: G05D 1/0246 using a video camera in combination with image processing means
• CPC Class Codes: G05D 1/0272 comprising means for registering the travel distance, e.g. revolutions of wheels measuring distance traversed on the ground by vehicles, e.g. using odometers G01C22/00
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30
• CPC Class Codes: G06F 30/18 Network design, e.g. design based on topological or interconnect aspects of utility systems, piping, heating ventilation air conditioning [HVAC] or cabling circuit design at the physical level G06F30/39; network planning tools for wireless communication networks H04W16/18
• CPC Class Codes: Y10S 706/905 Vehicle or aerospace
• CPC Class Codes: Y10S 706/919 Designing, planning, programming, CAD, CASE

Abstract

A method and apparatus for path planning are presented. Path planning involves propagating cost waves in a configuration space representation of a task space. A space variant metric and budding are used for cost wave propagation. The disclosed method and apparatus are readily adaptable to robots with n degrees of freedom.

Claims

1. A method for determining at least one physical motion specification for a physical object comprising executing the following steps in at least one digital data processing device and at least one computer readable storage medium that is included in or coupled with the at least one digital data processing device:

a) embodying, in the at least one computer readable storage medium, a configuration space data structure representing a physical task space that surrounds the physical object in physical reality, the configuration space data structure including signals representing the physical object and the physical task space; and

b) propagating cost waves, in the configuration space data structure, to fill the configuration space data structure with cost values according to a space variant metric, the cost values representing physical aspects of the physical task space with respect to physical motion of the physical object.

2. The method of claim 1, further comprising the steps of:

a. deriving a series of object pose representations within the configuration space data structure, using the cost values, which representations represent physical poses defining a least cost path from a start pose to a goal pose in the physical task space; and

b. providing the series in an electronic form usuable by the object to follow the path.

3. The method of claim 1 wherein

the method is a part of a method for controlling the object;

and

the method further comprises the step of controlling the object to follow the path.

4. A method for determining at least one physical motion specification for a physical object comprising executing the following steps in at least one digital data processing device and at least one computer readable storage medium that is included in or coupled with the at least one digital data processing device:

a) embodying, in the at least one computer readable storage medium, a configuration space data structure representing a physical task space that surrounds the physical object in physical reality, the configuration space data structure including signals representing the physical object and the physical task space environment, the configuration space data structure including a plurality of states which are data structures representing physical poses of the physical objects; and

b) propagating cost waves in the configuration space data structure, said propagating step comprising the steps of;

I) exploring all neighbors of a state in the configuration space data structure;

ii) improving neighbors of the state by assigning appropriate values of cost to goal and direction arrows to the state, which cost to goal and direction arrows values represent physical aspects of the physical task space and the physical motion; and

iii) adding improved neighbors to a storage data structure.

5. The method of claim 4 further comprising the steps of:

a. deriving a series of object pose representations within the configuration space data structure, using the cost values, which representations represent physical poses defining a least cost path from a start pose to a goal pose in the physical task space; and

b. providing the series in an electronic form usable by the object to follow the path.

6. A method as claimed in claim 4, further comprising the following steps

a. first transforming a physical goal pose into signals of at least one goal state in the configuration space data structure;

b. assigning cost and direction arrow values to states in the configuration space data structure so that each respective reachable state is assigned a cost value which represents the cost of an optimal path from the respective state to the goal state, and so that each respective reachable state is assigned at least one direction arrow value which represents a direction of travel on the optimal path;

c. second transforming the physical start pose into signals of a start state;

d. following the direction arrows from the start state to the goal state to obtain a series of object pose representations within the configuration space data structure, which representations represent physical poses defining a least cost path from the start pose to the goal pose in the physical task space.

7. The method of claim 6 comprising the steps of:

a) determining, after the second transforming step, whether the path exists, a path existing when there is at least one direction arrow value for the start state; and

b) stopping the method after the determining step, when no path exists.

8. The method of claim 6 further comprising, prior to the first providing step, the step of:

choosing a value which represents a coarseness of the configuration space data structure.

9. The method of claim 6 comprising the step of interactively choosing a set of possible direction arrows values.

10. The method of claim 6 wherein the assigning step comprises choosing the direction arrow values from a set comprising direction arrows values representing transitions parallel to axes of the configuration space corresponding to the configuration space data structure.

11. The method of claim 6 wherein the assigning step comprises choosing the direction arrow values from a set comprising direction arrow values representing transitions from a first state to respective ones of a plurality of second states, which second states are the visible states in an n-cube of p states on a side, the n-cube surrounding the first state, where n is a positive integer representing a number of degrees of freedom of the object and p is a positive integer.

12. The method of claim 6 wherein the assigning step includes measuring cost values of transitions between neighboring states with a metric.

13. The method of claim 12 comprising the step of inducing the metric derived from a criterion.

14. The method of claim 13 comprising the step of using the criterion of minimizing time of motion in the task space.

15. The method of claim 13 wherein the object is a robot with movable joints and comprising the step of using the criterion of minimizing joint motion of the robot.

16. The method of claim 13 comprising the step of using the criterion of minimizing distance of motion in the task space.

17. The method of claim 6 wherein the assigning step comprises evaluating cost according to a space variant metric function.

18. The method of claim 5 wherein the assigning step comprises evaluating cost according to a space variant metric function wherein constrained points in the physical task space are expressed as requiring higher cost transitions in the configuration space data structure.

19. The method of claim 6

comprising the further step of third transforming at least one further physical goal pose into corresponding further goal states in the configuration space data structure;

wherein the assigning step comprises assigning cost and direction arrow values which represent cost and direction of an optimal path from the respective state to a least cost one of the goal states; and

wherein the following step comprises following the direction arrows from the start state to the least cost one of the goal states.

20. The method of claim 6

comprising the step of third transforming at least one physical obstacle in the physical task space into at least one obstacle state in the configuration space data structure;

and

wherein the assigning step comprises assigning cost and direction arrows values so that each optimal path avoids the at least one obstacle state and so that the at least one obstacle state has no direction arrow values;

whereby the following step results in a path which avoids the obstacle.

21. The method of claim 20 wherein the third transforming step comprises assigning a substantially infinite value of cost to the obstacle state, whereby the obstacle state becomes part of a space variant metric function which measures cost.

22. The method of claim 6 wherein

the method is a part of a method for controlling the object;

and

the method further comprises the step of controlling the object to follow the path.

23. The method of claim 6 wherein the assigning step includes:

adding the states to a heap,

retrieving a top node representing a lowest cost node,

expanding the top node to determine the improved neighbors, and

adding the improved neighbors to the heap.

24. The method of claim 23 wherein the adding steps each include checking a flag in the configuration space data structure to determine if the state is already in the heap before the respective states or improved neighbors are added to the heap.

25. The method of claim 4 wherein

the method is a part of a method for controlling the object;

and

the method further comprises the step of controlling the object to follow the path.

26. Apparatus for determining at least one physical motion specification for a physical object, the apparatus comprising:

a. a memory for storing signals representing a discretized configuration space in the form of a configuration space data structure which includes a plurality of states corresponding to a discretized subset of all physical poses of the physical object in a physical task space, the states being arranged so that each state has a plurality of neighboring states situated along respective permissible transition directions;

b. means for initializing the states;

c. means for storing goal information, corresponding to a physical goal pose, in a respective goal state;

d. means for measuring cost of transition from each state to its neighboring states along the permissible directions according to a space variant metric function;

e. means for propagating cost waves from the goal state using the measuring means, the propagating means assigning to each state a cost value and a direction of travel corresponding to a physical least cost path from the physical pose corresponding to the state to the physical goal pose;

f. means for determining a series of discrete states along the physical path, the determining means determining the path states by starting at a start state corresponding to a physical start pose and following the cost and direction of travel values assigned to the start state and succeeding states; and

g. means for transforming the series into an electronic form usable by the physical object to follow the physical path.

27. The apparatus of claim 26 wherein:

a. the object is a robot; and

b. the discrete states along the path correspond to set points of the robot.

28. The apparatus of claim 26 wherein:

a. the object is an emergency vehicle;

b. the discrete states along the path correspond to street locations to which the emergency vehicle should travel; and

c. the apparatus comprises means for transmitting the discrete states to a driver of the emergency vehicle.

29. The apparatus of claim 26 wherein:

a. the apparatus is an emergency alarm system in a building;

b. the object is a person trying to escape from the building;

c. the path represents a safest way out of an emergency situation; and

d. the apparatus comprises means for communicating the path to the person.

30. The apparatus of claim 26 wherein:

a. the apparatus is an electronic map;

b. the object is a person attempting to find a route in a new area; and

c. the discrete states correspond to street locations to which the person should travel.

31. The apparatus of claim 26 wherein

the apparatus is part of a controller for the object;

and

the apparatus further comprises means for controlling the object to follow the path.

32. Apparatus for planning a physical least cost path comprising:

a) at least one computer readable storage medium for storing signals embodying a discretized data structure representation of a physical task space, which physical task space surrounds the physical object in physical reality;

b) means for assigning at least one respective cost to at least one neighboring position of any given position, based on I) a cost assigned to the given position; and

ii) a measure which varies according to position within the discretized representation, the cost corresponding to at least one physical aspect of the physical task space, so that a physical least cost path from the neighboring position to the given position is established;

c) means for starting the assigning means at a first position of known cost;

d) means for causing the assigning means to iterate, so that all positions within the discretized representation are assigned respective costs, in waves propagating outward from the first position; and

e) means for identifying a least cost path between two positions in the discretized representation based on the respective costs.