System and method for trajectory planning for manipulators in robotic finishing applications

1. A method for automatically moving a tool attached to a robotic manipulator from a start position to a goal position, the method comprising:

determining, using a processor, a plurality of next possible positions from the start position;

selecting, using the processor, a second position from the plurality of next possible positions based on respective costs associated with moving the tool from the start position to each of the possible positions in the plurality of next possible positions, the selecting of the second position being based on a determination of whether to use a first heuristic or a second heuristic;

moving, using a plurality of actuators, the tool to the second position; and

determining, using the processor, an updated plurality of next possible positions, selecting a next position from the updated plurality of next possible positions based on respective costs associated with moving the tool from the start position to each of the possible positions in the updated plurality of next possible positions, and moving the tool to the next position until the goal position is reached.

2. The method of claim 1 , wherein the plurality of next possible positions and the updated plurality of next possible positions are determined using a motion primitive, the motion primitive chosen from a plurality of possible motion primitives based on a freedom of motion of the robotic manipulator.

3. The method of claim 1 , wherein the first heuristic is a configuration space heuristic or a workspace heuristic and the second heuristic is an other of the configuration space heuristic or the workspace heuristic,

wherein the configuration space heuristic guides selection in configuration space, and

wherein the workspace heuristic guides selection in workspace.

4. The method of claim 3 , wherein selecting the second position from the plurality of next possible positions is further based on a diversity function indicating a diversity of next positions of each of the next possible positions.

5. The method of claim 3 , wherein an estimated cost of moving from the current position to the goal position in workspace is based on a distance cost associated with moving from the current position to the goal position in workspace and a rotational cost associated with moving from the current position to the goal position in workspace.

6. The method of claim 3 , further comprising maintaining, by the processor, a progress using the first heuristic, the progress being determined by based on at least a moving average of heuristic cost; and

switching to the second heuristic to determine the cost associated with moving the tool when the progress using the first heuristic is below a threshold value, the second heuristic being the configuration space heuristic when the first heuristic is the workspace heuristic, or the second heuristic being the workspace heuristic when the first heuristic is the configuration space heuristic.

7. The method of claim 6 , further comprising switching to the second heuristic based on a freedom of motion of the robotic manipulator.

8. The method of claim 1 , wherein an amount the tool is moved is determined based on congestion surrounding a current position of the tool, the amount being larger as the congestion surrounding the current position of the tool is higher, and the amount being smaller as the congestion surrounding the current position of the tool is lower.

9. A system for automatically moving a tool attached to a robotic manipulator from a start position to a goal position, the system comprising:

a processor configured to:

determine a plurality of next possible positions from the start position,

select a second position from the plurality of next possible positions based on respective costs associated with moving the tool from the start position to each of the possible positions in the plurality of next possible positions, the selecting of the second position being based on a determination of whether to use a first heuristic or a second heuristic, and

determine an updated plurality of next possible positions and select a next position from the updated plurality of next possible positions based on respective costs associated with moving the tool from the start position to each of the possible positions in the updated plurality of next possible positions until the goal position is reached; and

a plurality of actuators configured to move the tool to the second position, the next position, or the goal position.

10. The system of claim 9 , wherein the plurality of next possible positions and the updated plurality of next possible positions are determined by the processor using a motion primitive, the motion primitive chosen from a plurality of possible motion primitives based on a freedom of motion of the robotic manipulator.

11. The system of claim 9 , wherein the first heuristic is a configuration space heuristic or a workspace heuristic and the second heuristic is an other of the configuration space heuristic or the workspace heuristic,

wherein the configuration space heuristic guides selection in configuration space, and

wherein the workspace heuristic guides selection in workspace.

12. The system of claim 11 , wherein selection of the second position from the plurality of next possible positions by the processor is further based on a diversity function indicating a diversity of next positions of each of the next possible positions.

13. The system of claim 11 , wherein an estimated cost associated with moving from the current position to the goal position in workspace is based on a distance cost associated with moving from the current position to the goal position in workspace and a rotational cost associated with moving from the current position to the goal position in workspace.

14. The system of claim 11 , wherein the processor is further configured to:

maintain a progress using the first heuristic, the progress being determined based on at least a moving average of heuristic cost and

switch to the second heuristic to determine the cost associated with moving the tool when the progress using the first heuristic is below a threshold value, the second heuristic being the configuration space heuristic when the first heuristic is the workspace heuristic, or the second heuristic being the workspace heuristic when the first heuristic is the configuration space heuristic.

15. The system of claim 14 , wherein the processor is further configured to switch to the second heuristic based on a freedom of motion of the robotic manipulator.

16. The method of claim 9 , wherein the processor is further configured to determine an amount the tool is moved based on congestion surrounding a current position of the tool, the amount being larger as the congestion surrounding the current position of the tool is higher, and the amount being smaller as the congestion surrounding the current position of the tool is lower.

17. A method for automatically moving a tool attached to a robotic manipulator from a start position to a goal position, the method comprising:

determining, using a processor, a plurality of next possible positions from the start position using a motion primitive, the motion primitive chosen from a plurality of possible motion primitives based on a freedom of motion of the robotic manipulator;

selecting, using the processor, a second position from the plurality of next possible positions based on respective costs associated with moving the tool from the start position to each of the possible positions in the plurality of next possible positions, the selecting of the second position being based on a determination of whether to use a first heuristic or a second heuristic;

moving, using a plurality of actuators, the tool to the second position; and

determining, using the processor, an updated plurality of next possible positions, selecting a next position from the updated plurality of next possible positions based on respective costs associated with moving the tool from the start position to each of the possible positions in the updated plurality of next possible positions, and moving the tool to the next position until the goal position is reached.

18. The method of claim 17 , wherein the first heuristic is chosen from plurality of heuristics, each heuristic in the plurality of heuristics being a weighted combination of a configuration space heuristic, a workspace heuristic, and/or a diversity function,

wherein the configuration space heuristic guides selection in configuration space,

wherein the workspace heuristic guides selection in workspace, and

wherein the diversity function indicates a diversity of next positions of each of the next possible positions.

19. The method of claim 18 , wherein an estimated cost associated with moving from the current position to the goal position in workspace is based on a distance cost associated with moving from the current position to the goal position in workspace and a rotational cost associated with moving from the current position to the goal position in workspace.

20. The method of claim 18 , further comprising maintaining, by the processor, a progress using the first heuristic, the progress being determined based on at least a moving average of heuristic cost; and

switching to the second heuristic from the plurality of heuristics to determine the cost associated with moving the tool when the progress using the first heuristic is below a threshold value.