Systems and methods for precise navigation of autonomous devices
The safe operation and navigation of robots is an active research topic for many real-world applications, such as the automation of large industrial equipment. This technological field often requires heavy machines with arbitrary shapes to navigate very close to obstacles, a challenging and largely unsolved problem. To address this issue, a new planning architecture is developed that allows wheeled vehicles to navigate safely and without human supervision in cluttered environments. The inventive methods and systems disclosed herein belong to the Model Predictive Control (MPC) family of local planning algorithms. The technological features disclosed herein works in the space of two-dimensional (2D) occupancy grids and plans in motor command space using a black box forward model for state inference. Compared to the conventional methods and systems, the inventive methods and systems disclosed herein include several properties that make it scalable and applicable to a production environment. The inventive concepts disclosed herein are at least deterministic, computationally efficient, run in constant time and can be deployed in many common non-holonomic systems.
1. A system for causing a robot to navigate a space that includes obstacles, comprising:
at least one processor; and
a non-transitory computer readable media having executable instructions stored therein, which, when executed by at least one processor, cause the system to perform operations comprising:
accessing obstacle positions, a reference path through the space, and a plurality of motion primitives, each motion primitive associated with one of a plurality of pre-computed costs and a pre-computed control command;
generating a geometric trajectory cost of the robot based on the obstacle positions and the reference path;
generating a non-geometric trajectory cost of the robot;
selecting a single motion primitive from the plurality of motion primitives by minimizing a cost function based on the geometric trajectory cost, the non-geometric trajectory cost, and the pre-computed costs; and
providing the pre-computed control command associated with the single motion primitive to at least one processor for instructing one or more drive mechanism to move the robot according to the pre-computed control command.
2. The system of claim 1 , wherein accessing obstacle positions includes accessing a cost map of the space, further wherein minimizing the cost function includes applying, according to a state trajectory sequence, a trajectory footprint mask to a portion of the cost map having the same size and shape as the trajectory footprint mask, wherein the trajectory footprint mask has a shape and size defined by the precise shape and size of the robot relative to the obstacle positions and one or more obstacles are represented in the cost map as areas within the space having increased cost relative to areas within the space adjacent to the reference path.
3. The system of claim 1 , further comprising generating the pre-computed costs offline, wherein the accessing, generating, generating, selecting, and providing steps are performed online.
4. The system of claim 3 , wherein generating the pre-computed costs offline includes (i) generating the pre-computed costs while the robot is not navigating or (ii) generating the pre-computed costs while the robot is navigating using one or more of the at least one processor that is not performing the accessing, generating, generating, selecting, and providing steps.
5. The system of claim 1 , further comprising generating the pre-computed costs according to a rapidly-exploring random tree algorithm that sums costs of a plurality of control commands of each of the plurality of motion primitives for a plurality of initial states or initial trajectories.
6. The system of claim 1 , further comprising:
generating a second geometric trajectory cost of the robot based on the obstacle positions and the reference path;
generating a second non-geometric trajectory cost of the robot;
selecting a second single motion primitive from the plurality of motion primitives by minimizing the cost function based on the second geometric trajectory cost, the second non-geometric trajectory cost, and the pre-computed costs; and
when minimizing the cost function obtains a minimum cost that exceeds a threshold causing the robot controller to instruct the one or more drive mechanisms to stop the robot.
7. The system of claim 1 , wherein accessing the plurality of motion primitives includes selecting, based on an initial state of the robot, the plurality of motion primitives from a library of motion primitives.
8. A method for causing a robot to navigate a space that includes obstacles, comprising:
accessing obstacle positions, a reference path through the space, and a plurality of motion primitives, each motion primitive is associated with a pre-computed cost of executing a pre-computed control command;
generating a geometric trajectory cost of the robot based on the obstacle positions and the reference path;
generating a non-geometric trajectory cost of the robot;
selecting a single motion primitive from the plurality of motion primitives by minimizing a cost function based on the geometric trajectory cost, the non-geometric trajectory cost, and the pre-computed cost; and
providing the pre-computed control command associated with the single motion primitive to a controller configured for instructing one or more drive mechanism to move the robot according to the pre-computed control command.
9. The method of claim 8 , wherein,
the accessing of obstacle positions comprises accessing a cost map of the space,
the minimizing the cost function comprises applying, according to a state trajectory sequence, a trajectory footprint mask to a portion of the cost map having the same size and shape as the trajectory footprint mask, the trajectory footprint mask comprises a shape and size defined by the precise shape and size of the robot in relation to the obstacle positions and one or more obstacles are represented in the cost map as areas within the space having increased cost relative to areas within the space adjacent to the reference path.
10. The method of claim 8 , further comprising generating the pre-computed cost according to a rapidly-exploring random tree algorithm that sums costs of a plurality of control commands of each of the plurality of motion primitives for a plurality of initial states or initial trajectories.
11. The method of claim 8 , wherein accessing the plurality of motion primitives includes selecting, based on an initial state of the robot, the plurality of motion primitives from a library of motion primitives.
12. The method of claim 8 , further comprising generating the pre-computed cost offline, wherein the accessing, generating, generating, selecting, and providing steps are performed online.
13. The method of claim 12 , wherein the generating the pre-computed cost offline includes (i) generating the pre-computed cost while the robot is not navigating or (ii) generating the pre-computed cost while the robot is navigating using one or more of at least one processor that is not performing the accessing, generating, generating, selecting, and providing steps.
14. A non-transitory computer readable media having executable instructions stored therein, which, when executed by at least one processor coupled to a robot, cause the at least one processor to:
access obstacle positions, a reference path through a space, and a plurality of motion primitives, each motion primitive is associated with a pre-computed cost of executing a pre-computed control command;
generate a geometric trajectory cost of the robot based on the obstacle positions and the reference path;
generate a non-geometric trajectory cost of the robot;
select a single motion primitive from the plurality of motion primitives by minimizing a cost function based on the geometric trajectory cost, the non-geometric trajectory cost, and the pre-computed cost; and
provide the pre-computed control command to the at least one processor for instructing one or more drive mechanism of the robot to move the robot according to the pre-computed control command.
15. The non-transitory computer readable media of claim 14 , wherein accessing obstacle positions includes accessing, by the at least one processor executing the instructions, a cost map of the space, further wherein minimizing the cost function includes applying, according to a state trajectory sequence, a trajectory footprint mask to a portion of the cost map having the same size and shape as the trajectory footprint mask, wherein the trajectory footprint mask has a shape and size defined by the precise shape and size of the robot relative to the obstacle positions and one or more obstacles are represented in the cost map as areas within the space having increased cost relative to areas within the space adjacent to the reference path.
16. The non-transitory computer readable media of claim 14 , further comprising instructions which cause the at least one processor to generate pre-computed cost according to a rapidly-exploring random tree algorithm that sums costs of a plurality of control commands of each of the plurality of motion primitives for a plurality of initial states or initial trajectories.
17. The non-transitory computer readable media of claim 14 , wherein accessing the plurality of motion primitives includes the at least one processor executing the instructions to select, based on an initial state of the robot, the plurality of motion primitives from a library of motion primitives.
18. The non-transitory computer readable media of claim 14 , further comprising instructions which cause the at least one processor to generate the pre-computed cost offline, wherein the accessing, generating, generating, selecting, and providing steps are performed online.
19. The non-transitory computer readable media of claim 18 , wherein generating the pre-computed cost offline includes (i) generating pre-computed cost while the robot is not navigating or (ii) generating pre-computed cost while the robot is navigating using one or more of the at least one processor that is not performing the accessing, generating, generating, selecting, and providing steps.