Method and apparatus for path planning

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.

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.

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.