CPC G01C 21/20 (2013.01) [G05D 1/0088 (2013.01); G05D 1/0214 (2013.01); G05D 2201/0217 (2013.01)] | 20 Claims |
1. A computer-implemented method when executed by data processing hardware of a robot causes the data processing hardware to perform operations comprising:
receiving image data of an environment of the robot from an image sensor;
receiving a body obstacle map comprising a map of obstacles that a body of the robot cannot traverse;
generating a sparse graph based on the body obstacle map, the sparse graph comprising a set of nodes and edges that begin at a starting location and end at a destination, the nodes and edges representative of paths the robot may travel in the environment;
planning a coarse path from the starting location to the destination;
determining a point along the coarse path between a first node of the set of nodes and a second node of the set of nodes where line of sight by the image sensor is lost;
generating an intermediate waypoint at the point along the coarse path where line of sight by the image sensor is lost; and
navigating the robot from the starting location along the coarse path and the intermediate waypoint toward the destination.
|