Computer-implemented method, robot and system

Abstract

Computer-implemented methods, robots, and systems are provided. The computer-implemented method includes receiving a navigation route at data processing hardware of the robot, the navigation route including a first waypoint and a second waypoint; receiving, at the data processing hardware, image data associated with an environment of the robot from an image sensor; determining, by the data processing hardware, a plurality of paths from the first waypoint to the second waypoint based on the image data; selecting, by the data processing hardware, a path from the plurality of paths, the path comprising at least one intermediate path point; and commanding, by the data processing hardware, navigation of the robot from a starting location to a destination location in the environment via the first waypoint, the at least one intermediate waypoint, and the second waypoint.

Description

[0001] This patent application is a divisional application of the following invention patent application:

[0002] Application No.: 201980098897.7

[0003] Filing Date: September 13, 2019

[0004] Title of Invention: Intermediate Waypoint Generator Technical Field

[0005] The present disclosure relates to generating intermediate waypoints in the presence of constraints, particularly those imposed by terrain. Background Art

[0006] Robotic devices are increasingly being used to navigate in constrained environments to perform various tasks or functions. These robotic devices often need to navigate through these constrained environments without contacting obstacles or getting stuck or trapped. As these robotic devices become more prevalent, there is a need for real-time navigation and route planning that avoids contact with obstacles while successfully navigating to a destination. Summary of the Invention

[0007] One aspect of the present disclosure provides a method for generating intermediate waypoints for a navigation system of a robot. The method includes receiving a navigation route at a data processing hardware of the robot. The navigation route includes a series of high-level waypoints that start at a starting position and end at a destination position and are based on high-level navigation data. The high-level navigation data represents the positions of static obstacles in the area where the robot is to navigate. The method further includes receiving, at the data processing hardware, image data of the environment around the robot from an image sensor. The method further includes generating, by the data processing hardware, at least one intermediate waypoint based on the image data. The method further includes adding, by the data processing hardware, the at least one intermediate waypoint to the series of high-level waypoints of the navigation route, and navigating, by the data processing hardware, the robot from the starting position along the series of high-level waypoints and the at least one intermediate waypoint toward the destination position.

[0008] Embodiments of the present disclosure may include one or more of the following optional features. In some embodiments, each high-level path point and each intermediate path point include two coordinates indicating a position on a plane, a yaw value, and a time value. The time value indicates an estimated amount of time for the robot to navigate to the corresponding path point. In some examples, the method further includes maintaining, by data processing hardware, each high-level path point in the series of high-level path points on the navigation route. The method may further include receiving, at the data processing hardware, a body obstacle map. The body obstacle map includes a map of obstacles that the body of the robot cannot cross. Additionally, generating at least one intermediate path point may include generating a sparse map based on the body obstacle map. The sparse map includes a series of nodes and edges. The nodes and edges represent paths that the robot can travel in the environment. The method may further include planning a rough path from a first node from the series of nodes to a second node from the series of nodes. The first node and the second node each represent a space in the environment. The method may further include determining a point at which a line of sight of an image sensor is lost along the rough path, and generating one of at least one intermediate path point at the point where the line of sight is lost.

[0009] In some embodiments, generating the sparse map includes generating a fully configured space map based on the body obstacle map. The fully configured space map includes a two-dimensional grid of elements, where each element of the grid represents a space in the environment and each element of the fully configured space map includes a corresponding set of yaw configurations. Each yaw configuration may be classified as valid or invalid, where a valid yaw configuration represents a yaw configuration of the robot that is free from the risk of contacting an obstacle at the space associated with the corresponding element, and an invalid yaw configuration represents a yaw configuration of the robot that is not free from the risk of contacting an obstacle at the space associated with the corresponding element. The method may further include generating a compressed configured space map from the fully configured space map. The compressed configured space map includes a second two-dimensional grid of elements, and each element of the second grid represents a space in the environment. Each element of the second grid is also classified as one of the following: (i) a yaw collision zone, (ii) a yaw free zone, or (iii) a yaw constraint zone. In some examples, the method includes generating a sparse map from the compressed configured space map.

[0010] Optionally, planning a rough path from a first node to a second node includes generating a dense map from the compressed configured space map. The dense map includes elements classified as yaw free zones. The method may further include linking edges from the sparse map to elements from the dense map. In some embodiments, linking the edges to elements from the dense map includes combining the sparse map and the dense map to generate a final map. In some examples, the method further includes performing an A* search algorithm on the final map.

[0011] In some examples, generating the sparse graph further includes overlapping a plurality of Voronoi cells on the compressed configuration space map, where each Voronoi cell is classified as a yaw constraint region on the sparse graph. Each Voronoi cell is equidistant from at least two elements classified as yaw collision regions. Optionally, generating the sparse graph further includes classifying each Voronoi cell as either an edge or a node. Classifying each Voronoi cell may include performing a flood fill algorithm.

[0012] In some embodiments, planning a rough path from a first node to a second node includes pruning edges, and each pruned edge includes elements that are below a threshold length and are classified as either a yaw collision region or a yaw constraint region. Determining a point where the line of sight of an image sensor is lost along the rough path may include determining a minimum allowable yaw and a maximum allowable yaw at each element along the planned rough path, and determining a minimum envelope based on the minimum allowable yaw and the maximum allowable yaw. The method may further include determining that a required yaw at a point on the rough path is outside the minimum envelope.

[0013] In some examples, the method further includes sending, by the data processing hardware, a navigation route having high-level path points and at least one intermediate path point to a low-level path generator. The method may further include determining, by the data processing hardware, whether to add the at least one intermediate path point to the navigation route, and in response to determining not to add the at least one intermediate path point to the navigation route, passing, by the data processing hardware, the unchanged navigation route to the low-level path generator.

[0014] Another aspect of the present disclosure provides a robot that includes a body and legs coupled to the body and configured to maneuver the robot in an environment. The robot further includes data processing hardware in communication with the legs and memory hardware in communication with the data processing hardware. The memory hardware stores instructions that, when executed on the data processing hardware, cause the data processing hardware to perform operations. The operations include receiving a navigation route. The navigation route includes a series of high-level path points that begin at a starting position and end at a destination position and are based on high-level navigation data. The high-level navigation data represents the positions of static obstacles in the area in which the robot is to navigate. The operations further include receiving image data of the environment around the robot from an image sensor. The operations further include generating at least one intermediate path point based on the image data. The operations further include adding the at least one intermediate path point to the series of high-level path points of the navigation route, and navigating the robot from the starting position along the series of high-level path points and the at least one intermediate path point toward the destination position.

[0015] This aspect may include one or more of the following optional features. In some embodiments, each high-level waypoint and each intermediate waypoint include two coordinates that indicate a position on a plane, a yaw value, and a time value. The time value indicates an estimated amount of time for the robot to navigate to the corresponding waypoint. In some examples, the operation also includes maintaining each high-level waypoint in the series of high-level waypoints on the navigation route. The operation may also include receiving a body obstacle map. The body obstacle map includes a map of obstacles that the body of the robot cannot cross. Additionally, generating at least one intermediate waypoint may include generating a sparse map based on the body obstacle map. The sparse map includes a series of nodes and edges. The nodes and edges represent paths that the robot can travel in the environment. The operation may also include planning a rough path from a first node from a series of nodes to a second node from the series of nodes. The first node and the second node each represent a space in the environment. The operation may also include determining a point where the line of sight of an image sensor is lost along the rough path and generating one of at least one intermediate waypoint at the point where the line of sight is lost.

[0016] In some embodiments, generating the sparse map includes generating a fully configured space map based on the body obstacle map. The fully configured space map includes a two-dimensional grid of elements, where each element of the grid represents a space in the environment and each element of the fully configured space map includes a corresponding set of yaw configurations. Each yaw configuration can be classified as valid or invalid, where a valid yaw configuration represents a yaw configuration of the robot that is free from the risk of contacting an obstacle at the space associated with the corresponding element, and an invalid yaw configuration represents a yaw configuration of the robot that is not free from the risk of contacting an obstacle at the space associated with the corresponding element. The operation may also include generating a compressed configured space map from the fully configured space map. The compressed configured space map includes a second two-dimensional grid of elements, and each element of the second grid represents a space in the environment. Each element of the second grid is also classified as one of the following: (i) a yaw collision zone, (ii) a yaw free zone, or (iii) a yaw constraint zone. In some examples, the operation includes generating a sparse map from the compressed configured space map.

[0017] Optionally, planning a rough path from the first node to the second node includes generating a dense map from the compressed configured space map. The dense map includes elements classified as yaw free zones. The operation may also include linking the edges from the sparse map to the elements from the dense map. In some embodiments, linking the edges to the elements from the dense map includes combining the sparse map and the dense map to generate a final map. In some examples, the method also includes performing an A* search algorithm on the final map.

[0018] In some examples, generating the sparse graph further includes overlapping a plurality of Voronoi cells on the compressed configuration space map, where each Voronoi cell is classified as a yaw constraint region on the sparse graph. Each Voronoi cell is equidistant from at least two elements classified as yaw collision regions. Optionally, generating the sparse graph further includes classifying each Voronoi cell as either an edge or a node. Classifying each Voronoi cell may include performing a flood fill algorithm.

[0019] In some embodiments, planning a coarse path from a first node to a second node includes pruning edges, and each pruned edge includes an element that is below a threshold length and is classified as either a yaw collision region or a yaw constraint region. Determining a point where the line of sight of an image sensor is lost along the coarse path may include determining a minimum allowable yaw and a maximum allowable yaw at each element along the planned coarse path, and determining a minimum envelope based on the minimum allowable yaw and the maximum allowable yaw. The operation may further include determining that the required yaw at a point on the coarse path is outside the minimum envelope.

[0020] In some examples, the operation further includes sending a navigation route having a high-level path point and at least one intermediate path point to a low-level path generator. The operation may further include determining whether to add at least one intermediate path point to the navigation route, and in response to determining not to add the at least one intermediate path point to the navigation route, passing the unchanged navigation route to the low-level path generator.

[0021] Another aspect of the present disclosure provides a computer-implemented method, including: receiving, at a data processing hardware of a robot, a navigation route, the navigation route including a first path point and a second path point; receiving, at the data processing hardware, image data associated with an environment of the robot from an image sensor; determining, by the data processing hardware, a plurality of paths from the first path point to the second path point based on the image data; selecting, by the data processing hardware, a path from the plurality of paths, the path including at least one intermediate path point; and commanding, by the data processing hardware, the robot to navigate from a starting position to a destination position in the environment via the first path point, the at least one intermediate path point, and the second path point.

[0022] Another aspect of the present disclosure provides a system, comprising: data processing hardware; and a memory communicatively coupled to the data processing hardware, the memory storing instructions, wherein the instructions, when executed by the data processing hardware, cause the data processing hardware to: receive a navigation route that includes a first waypoint and a second waypoint; receive image data associated with the environment of the robot from an image sensor; determine, based on the image data, multiple paths from the first waypoint to the second waypoint; select a path from the multiple paths, the path including at least one intermediate waypoint; and command the robot to navigate from a starting position to a destination position in the environment via the first waypoint, the at least one intermediate waypoint, and the second waypoint.

[0023] Another aspect of the present disclosure provides a robot, comprising: an image sensor; at least two legs; data processing hardware communicatively coupled to the image sensor; and a memory communicatively coupled to the data processing hardware, the memory storing instructions, wherein the instructions, when executed by the data processing hardware, cause the data processing hardware to: receive a navigation route that includes a first waypoint and a second waypoint; receive image data associated with the environment of the robot from the image sensor; determine, based on the image data, multiple paths from the first waypoint to the second waypoint; select a path from the multiple paths, the path including at least one intermediate waypoint; and command the robot to navigate from a starting position to a destination position in the environment via the first waypoint, the at least one intermediate waypoint, and the second waypoint.

[0024] Another aspect of the present disclosure provides a computer-implemented method that, when executed by the data processing hardware of a robot, causes the data processing hardware to perform operations, including: receiving image data of the environment of the robot from an image sensor; receiving a body obstacle map that includes a map of obstacles that the body of the robot cannot cross; generating a sparse graph based on the body obstacle map, the sparse graph including a set of nodes and edges that start at a starting position and end at a destination, the nodes and edges representing paths that the robot can travel in the environment; planning a rough path from the starting position to the destination; determining points along the rough path between a first node in the set of nodes and a second node in the set of nodes where the line of sight of the image sensor is lost; generating intermediate waypoints at the points along the rough path where the line of sight of the image sensor is lost; and navigating the robot from the starting position towards the destination along the rough path and the intermediate waypoints.

[0025] Another aspect of the present disclosure provides a robot, comprising: a body; legs coupled to the body and configured to maneuver the robot in an environment; data processing hardware in communication with the legs; and memory hardware in communication with the data processing hardware, the memory hardware storing instructions that, when executed on the data processing hardware, cause the data processing hardware to perform operations including: receiving image data of the robot's environment from an image sensor; receiving a body obstacle map, the body obstacle map including a map of obstacles that the robot's body cannot cross; generating a sparse map based on the body obstacle map, the sparse map including a set of nodes and edges starting at a starting position and ending at a destination, the nodes and edges representing paths that the robot can travel in the environment; planning a rough path from the starting position to the destination; determining a point where the line of sight of the image sensor is lost along the rough path between a first node in the set of nodes and a second node in the set of nodes; generating an intermediate path point at the point where the line of sight of the image sensor is lost along the rough path; and navigating the robot from the starting position towards the destination along the rough path and the intermediate path point.

[0026] Another aspect of the present disclosure provides a method, comprising: receiving, at data processing hardware of a robot, a navigation route, the navigation route including a series of high-level path points starting at a starting position and ending at a destination position, and the navigation route being based on high-level navigation data representing the positions of static obstacles in the area in which the robot is to navigate; receiving, at the data processing hardware, image data of the environment around the robot from an image sensor; generating, by the data processing hardware, at least one intermediate path point based on the image data; adding, by the data processing hardware, the at least one intermediate path point to the series of high-level path points of the navigation route; and navigating, by the data processing hardware, the robot from the starting position towards the destination position along the series of high-level path points and the at least one intermediate path point, wherein each high-level path point and each intermediate path point of the at least one intermediate path point includes two coordinates indicating a position on a plane, a yaw value, and a time value, the time value indicating an estimated amount of time for the robot to navigate to the corresponding path point.

[0027] Another aspect of the present disclosure provides a method, comprising: receiving, at a data processing hardware of a robot, a navigation route that includes a series of high-level waypoints starting at a starting position and ending at a destination position, and the navigation route is based on high-level navigation data representing positions of static obstacles in an area where the robot is to navigate; receiving, at the data processing hardware, image data of an environment around the robot from an image sensor; generating, by the data processing hardware, at least one intermediate waypoint based on the image data; adding, by the data processing hardware, the at least one intermediate waypoint to the series of high-level waypoints of the navigation route; navigating, by the data processing hardware, the robot from the starting position toward the destination position along the series of high-level waypoints and the at least one intermediate waypoint; sending, by the data processing hardware, the navigation route having the high-level waypoints and the at least one intermediate waypoint to a low-level path generator; determining, by the data processing hardware, whether to add the at least one intermediate waypoint to the navigation route; and in response to determining not to add the at least one intermediate waypoint to the navigation route, passing, by the data processing hardware, the unchanged navigation route to the low-level path generator.

[0028] Another aspect of the present disclosure provides a robot, comprising: a body; legs coupled to the body and configured to maneuver the robot in an environment; data processing hardware in communication with the legs; and memory hardware in communication with the data processing hardware, the memory hardware storing instructions that, when executed on the data processing hardware, cause the data processing hardware to perform operations including: receiving a navigation route that includes a series of high-level waypoints starting at a starting position and ending at a destination position, and the navigation route is based on high-level navigation data representing positions of static obstacles in an area where the robot is to navigate; receiving image data of an environment around the robot from an image sensor; generating at least one intermediate waypoint based on the image data; adding the at least one intermediate waypoint to the series of high-level waypoints of the navigation route; and navigating the robot from the starting position toward the destination position along the series of high-level waypoints and the at least one intermediate waypoint, wherein each of the high-level waypoints and each of the at least one intermediate waypoints includes: two coordinates indicating a position on a plane, a yaw value, and a time value indicating an estimated amount of time for the robot to navigate to the corresponding waypoint.

[0029] Another aspect of the present disclosure provides a robot, comprising: a body; legs coupled to the body and configured to maneuver the robot in an environment; data processing hardware in communication with the legs; and memory hardware in communication with the data processing hardware, the memory hardware storing instructions which, when executed on the data processing hardware, cause the data processing hardware to perform operations including: receiving a navigation route that includes a series of high-level waypoints starting at a starting position and ending at a destination position, and the navigation route being based on high-level navigation data representing the positions of static obstacles in the area in which the robot is to navigate; receiving image data of the environment around the robot from an image sensor; generating at least one intermediate waypoint based on the image data; adding the at least one intermediate waypoint to the series of high-level waypoints of the navigation route; navigating the robot from the starting position towards the destination position along the series of high-level waypoints and the at least one intermediate waypoint; determining whether to add the at least one intermediate waypoint to the navigation route; and in response to determining not to add the at least one intermediate waypoint to the navigation route, passing the unchanged navigation route to a low-level path generator.

[0030] Details of one or more embodiments of the present disclosure are set forth in the accompanying drawings and the following description. Other aspects, features, and advantages will be apparent from the specification, drawings, and claims. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 is a schematic diagram of an example system of a navigation system for generating intermediate waypoints for a robot.

[0032] Figure 2A and Figure 2B is a schematic diagram of a robot navigating a navigation route using high-level waypoints.

[0033] Figure 3 is a robot navigating using high-level waypoints and intermediate waypoints Figure 2A and Figure 2B of a navigation route.

[0034] Figure 4 is Figure 1 a schematic diagram of an example intermediate waypoint generator of a navigation system.

[0035] Figure 5 is a schematic diagram of a compressed configuration space map.

[0036] Figure 6 is Figure 5 a schematic diagram of the configuration space map overlapping with Voronoi cells.

[0037] Figure 7 is Figure 6 a schematic diagram of a Voronoi cell, showing the distances between elements and the nearest obstacle of the Voronoi cell.

[0038] Figures 8A - 8E is the flood filling algorithm for filling Figure 6 the Voronoi cell.

[0039] Figure 9 is from Figure 6 the edges and nodes derived from the Voronoi cell.

[0040] Figure 10 is from Figure 6 the trimmed edges and nodes derived from the Voronoi cell.

[0041] Figure 11A and Figure 11B are schematic diagrams of a two-dimensional sparse graph ( Figure 11A ) and an SE2 sparse graph ( Figure 11B ).

[0042] Figure 12A and Figure 12B are schematic diagrams of the added nodes for the target destination.

[0043] Figure 13 is a schematic diagram of a path passing through a yaw-free zone and a yaw-constrained zone.

[0044] Figure 14 is a schematic diagram of a path passing through a yaw-constrained zone.

[0045] Figure 15A and Figure 15B are schematic diagrams of the navigation route of a robot, which includes intermediate waypoints placed at positions where the line of sight is lost.

[0046] Figure 16 is a flowchart of an example method for generating intermediate waypoints of a navigation system of a robot.

[0047] Figure 17 is a schematic diagram of an example computing device that can be used to implement the systems and methods described herein.

[0048] Like reference numerals in different figures represent like elements. DETAILED DESCRIPTION

[0049] As legged robotic devices (also referred to as "robots") become more prevalent, there is an increasing need for robots to autonomously navigate in environments that are constrained in many ways. Generally, robots rely on high-level map data that stores information related to large and / or static objects (e.g., walls, doors, etc.). When given a destination goal, a robot will typically first draw a path or route from this high-level map to navigate around static obstacles and then rely on a perception system that collects low-level navigation data to navigate around small and dynamic objects encountered during travel. Generally, when navigating in such environments, a robot will encounter obstacles that are not accounted for in the high-level map but are too large for the low-level system to navigate around. In these cases, it is not uncommon for the robot to become stuck or trapped. Embodiments of the present disclosure are directed to systems and methods for an intermediate waypoint generator that generates intermediate waypoints to a high-level route in real time, thereby allowing a legged robotic device to effectively navigate around larger dynamic objects.

[0050] Reference Figure 1 , the robot or robotic device 10 includes a body 11 having two or more legs 12 and implements a navigation system 100 for enabling the robot 10 to navigate in a constrained environment 8. Each leg 12 is coupled to the body 11 and may have an upper portion 14 and a lower portion 16 separated by a leg joint 18. The lower portion 16 of each leg 12 terminates in a foot 19. The foot 19 of each leg is optional, and the terminal end of the lower portion of one or more legs 12 may be coupled to a wheel, or the distal end of each leg 12 may directly contact the ground 9. The robot 10 has a gravity axis Vg that is perpendicular to the direction of gravity and a center of mass CM, which is the point at which the weighted relative positions of the distributed mass of the robot 10 sum to zero. The robot 10 also has a pose P based on the CM relative to the vertical gravity axis Vg (i.e., relative to a fixed reference frame of gravity) to define a particular attitude or stance made by the robot 10. The attitude of the robot 10 may be defined by the orientation or angular position of the robot 10 in space. The movement of the legs 12 relative to the body 11 changes the pose P of the robot 10 (i.e., the combination of the position of the robot's CM and the attitude or orientation of the robot 10).

[0051] In some embodiments, the robot 10 further includes one or more attachments, such as an articulated arm 20 disposed on the body 11 and configured to move relative to the body 11. The articulated arm 20 may have five or more degrees of freedom. Additionally, the articulated arm 20 may be interchangeably referred to as a manipulator arm or simply as an attachment. In the example shown, the articulated arm 20 includes two portions 22, 24 that may rotate relative to each other and relative to the body 11; however, the articulated arm 20 may include more or fewer portions without departing from the scope of the present disclosure. The first portion 22 may be separated from the second portion 24 by an articulated arm joint 26. An end effector 28, which may be interchangeably referred to as a manipulator head 28, may be coupled to the end of the second portion 24 of the articulated arm 20 and may include one or more actuators 29 for gripping / grasping an object.

[0052] The robot 10 further includes a vision system 30 having at least one imaging sensor or camera 31, each sensor or camera 31 capturing image data or sensor data 17 of the environment 8 around the robot 10 at a viewing angle 32 and within a field of view 34. The vision system 30 may be configured to move the field of view 34 by adjusting the viewing angle 32 or by translating and / or tilting the camera 31 (either independently or via the robot 10) to move the field of view 34 in any direction. Alternatively, the vision system 30 may include multiple sensors or cameras 31 such that the vision system 30 captures a substantially 360-degree field of view around the robot 10.

[0053] In some embodiments, one or more cameras 31 of the vision system 30 include one or more stereo cameras (e.g., one or more RGBD stereo cameras). In other examples, the vision system 30 includes one or more radar sensors, such as a scanning light detection and ranging (LIDAR) sensor, or a scanning laser detection and ranging (LADAR) sensor, a light scanner, a time-of-flight sensor, or any other three-dimensional (3D) volumetric image sensor (or any combination of such sensors). In some embodiments, the navigation system 100 includes a perception system 102 that receives and processes the sensor data 17 and passes the processed sensor data 17 to an intermediate path point generator 120 and a low-level path generator 130.

[0054] The vision system 30 provides image data or sensor data 17 to the data processing hardware 36 of the robot 10, and the image data or sensor data 17 is derived from the image data captured by the camera or sensor 31. The data processing hardware 36 communicates digitally with the memory hardware 38, and in some embodiments, the data processing hardware 36 can be a remote system. The remote system can be a single computer, multiple computers, or a distributed system (e.g., a cloud environment) with scalable / elastic computing resources and / or storage resources. The navigation system 100 of the robot 10 is executed on the data processing hardware 36.

[0055] In the example shown, the navigation system 100 includes an advanced waypoint generator 110 that receives map data 50 (i.e., high-level navigation data representing the positions of static obstacles in the area where the robot 10 is to navigate) and generates one or more advanced waypoints 210 for the navigation route 112 ( Figure 2A and 2B ), and the navigation route 112 depicts a rough path that bypasses large and / or static obstacles from the current position of the robot 10 to the target destination. For example, the advanced waypoint generator 110 can generate a rough path on a scale greater than 10 meters (e.g., a distance greater than 10 meters from the robot 10). The navigation system 100 also includes an intermediate waypoint generator 120 that receives the navigation route 112 and the image or sensor data 17 from the vision system 30, and includes the intermediate waypoint generator 120. Based on the sensor data 17, the intermediate waypoint generator 120 can add one or more intermediate waypoints 310 ( Figure 3 to the advanced waypoints 210 ( Figure 2A 、 Figure 2B and Figure 3 ) of the navigation route 112, which form a rough path or trajectory for the robot 10 to travel. The advanced waypoints 210 of the navigation route 112 and any added intermediate waypoints 310 are passed to the low-level path generator 130, and the low-level path generator 130 generates a step plan 142 that depicts each individual step of the robot 10 in combination with the sensor data 17 to navigate from the current position of the robot 10 to the next waypoint 210, 310. Using the step plan 142, the robot 10 follows the step plan 142 by placing the end of the foot 19 or leg 12 at the position on the ground 9 indicated by the step plan 142, thereby maneuvering through the environment 8. In some embodiments, the low-level path generator 130 provides lower-level obstacle avoidance by using a potential field method to avoid obstacles on a scale of approximately four meters. The potential field method can "push" the robot 10 away from obstacles by integrating the sum of the velocity towards the target and the obstacle avoidance velocity.

[0056] In some embodiments, at least a portion of the navigation system 100 is executed on a remote device that communicates with the robot 10. For example, the high-level waypoint generator 110 can be executed on the remote device to generate one or more high-level waypoints 210, and the intermediate waypoint generator 120 executed on the robot 10 can receive the high-level waypoints 210 from the remote device. Optionally, the entire navigation system 100 can be executed on the remote device, and the remote device can control / direct the robot 10 to maneuver the environment 8 based on the footstep planning 142.

[0057] Now referring to Figure 2A and Figure 2B , the low-level path generator 130 (using, e.g., a potential field method) can encounter problems when navigating around obstacles of a particular shape and / or protrusions. For example, the low-level path generator 130 may slide down an obstacle or surface 220 and become stuck in a local minimum 230 when attempting to navigate to a high-level waypoint 210 of the corresponding navigation route 112 ( Figure 2A ). In another example, the low-level path generator 130 may lack the necessary context or decision-making ability to turn the robot 10. That is, the low-level path generator 130 may have difficulty distinguishing when to "turn then drive" versus "drive then turn" ( Figure 2B ).

[0058] Now referring to Figure 3 , adding additional waypoints (i.e., intermediate waypoints 310) to the navigation route 112 helps the low-level path generator 130 make appropriate topological decisions because the additional one or more waypoints 310 can make the trajectory convex to the low-level path generator 130 and allow the low-level path generator to successfully navigate around the obstacle 220. The intermediate waypoint generator 120 can ensure that any added intermediate waypoints 310 added to the navigation route 112 are at a sufficient distance from the robot 10 to minimize the effects of map jitter. Additionally, the intermediate waypoint generator 120 can avoid removing or deleting any high-level waypoints 210 because the high-level waypoint generator 110 may know additional map context that is not available to the intermediate waypoint generator 120, and removing high-level waypoints 210 may produce unpredictable results.

[0059] Now referring to Figure 4, in some embodiments, the intermediate waypoint generator 120 includes a sparse graph generator 410, a coarse path planner 420, and a waypoint placer 430. The sparse graph generator 410 receives a body obstacle map 412 from, for example, the perception system 102. The body obstacle map 412 is a map of obstacles (e.g., obstacles that the body 11 of the robot 10 cannot cross) in the immediate vicinity of the robot 10 (e.g., within four meters). The sparse graph generator 410 generates a sparse graph 416S from the body obstacle map 412. As discussed in more detail below, the sparse graph 416S provides a series of valid interconnections between nodes 812 ( Figures 8A - 8E ), and edges 810 ( Figures 8A - 8E ). The nodes 812 and edges 810 represent valid potential paths through the yaw constraint regions (i.e., regions of difficult or strict maneuvering) near the robot 10, and thus the sparse graph 416S provides the information needed to determine the potential paths for the robot 10 to cross the yaw constraint regions.

[0060] The sparse graph generator 410 passes the sparse graph 416S to the coarse path planner 420. The coarse path planner 420 uses the sparse graph 416S to draw a coarse path 1310 ( Figure 13 ) from the starting position to the target destination. In some examples, the coarse path planner 420 draws the coarse path 1310 by using a search algorithm (e.g., A*) to determine the best route through the valid potential paths (i.e., the interconnections between a series of nodes 812 and edges 810) indicated by the sparse graph 416S. The coarse path planner 420 may pass the path 1310 to the waypoint placer 430. The waypoint placer 430 determines the positions where line of sight is lost when traveling along the path 1310 based on the current position of the robot 10, and places intermediate waypoints at those positions. That is, if the robot 10 looks along the coarse path 1310 from the current position, the waypoint placer determines the positions where the robot 10 will lose the line of sight of the path (e.g., due to obstacles), and places the intermediate waypoints 310 at the positions where the line of sight is lost on the path 1310. The intermediate waypoints 310 will guide the robot 10 to positions where the low-level path generator 130 can successfully navigate the obstacles. The intermediate waypoints 310 are included in the navigation route 112 and are passed to the low-level path generator 130.

[0061] In some embodiments, the sparse map generator 410 initially generates a fully configured space map 414 based on the received body obstacle map 412. In some examples, the fully configured space map 414 includes a two-dimensional (2D) grid or array of elements 510, 510a–n, where each element 510 represents a region of the environment 8 (e.g., a 3 cm by 3 cm square) that indicates whether an obstacle prevents the robot 10 from entering a particular element 510. Optionally, the fully configured space map 414 is an array of 128 elements by 128 elements. In some examples, each element 510 may be associated with a plurality of corresponding yaw configurations 512, 512a–n (corresponding sets of yaw configurations 512, 512a–n), and the fully configured space map 414 may indicate the status of each corresponding yaw configuration 512 of each element 510. For example, each element 510 may be associated with 64 corresponding yaw configurations. In this example, each yaw configuration will rotate 5.625 degrees from the previous configuration because 360 degrees (i.e., a full circle) divided by 64 (i.e., the number of yaw configurations) equals 5.625 degrees. For each corresponding yaw configuration 512 associated with a corresponding element 510, the fully configured space map 414 may indicate whether the corresponding yaw configuration 512 will result in a collision with a body obstacle if the selected point of the robot 10 occupies the space represented by the element 510. That is, the fully configured space map 414 may indicate whether each yaw configuration 512 is valid (i.e., the robot 10 will not collide with an obstacle in this yaw configuration 512) or invalid (i.e., the robot 10 will collide with an obstacle in this yaw configuration 512). The selected point may, for example, include the center point of the body 11 of the robot 10 (e.g., equidistant from the top of each leg 12).

[0062] In some embodiments, the sparse map generator 410 generates a compressed configured space map 500 from the fully configured space map 414. Instead of associating each element with a plurality of yaw configurations 512, the compressed configured space map 500 may classify each element 510 of the configured space map 500. For example, each element 510 may be classified as a body obstacle element 510A ( Figure 5 ), a yaw collision element 510B ( Figure 5 ), a yaw free element 510C ( Figure 5 ), or a yaw constraint element 510D ( Figure 5 ). That is, the yaw configurations 512 from the fully configured space map 414 may be used to classify each element 510 of the compressed configured space map 500.

[0063] Now refer to Figure 5, an exemplary compressed configuration space map 500 is shown. In this example, the compressed configuration space map 500 includes a grid of 128 by 128 elements 510, each element being classified as a body obstacle element 510A, a yaw collision element 510B, a yaw free element 510C, or a yaw constraint element 510D. The body obstacle element 510A represents an element occupied by an obstacle that the robot 10 cannot cross (i.e., an obstacle received from the body obstacle map 412). For example, a chair, a box, or a wall that the robot 10 must avoid can be classified as a body obstacle element 510A. The yaw collision element 510B is an element in which when the selection point of the robot 10 (e.g., the center of the body 11) occupies the space represented by the element 510B, all yaw configurations 512 of the robot 10 (e.g., all 64 yaw configurations) result in a collision with a body obstacle (i.e., the body obstacle element 510A). For example, the space adjacent to a wall, although not actually containing a wall, may be too close to the wall for the robot 10 to stand without colliding with the wall.

[0064] The yaw free element 510C is the opposite of the yaw collision element 510B in that no yaw configuration 512 will result in a collision with a body obstacle. In other words, the yaw free element 510C represents an area in the environment 8 where the robot 10 can rotate through all yaw configurations 512 (i.e., rotate 360 degrees about the axis at the selection point) without colliding with a body obstacle. The yaw constraint element 510D is an element in which some yaw configurations 512 are safe or valid (i.e., the robot 10 will not collide with an obstacle), while some yaw configurations 512 are unsafe (i.e., invalid yaw configurations 512 in which the robot 10 will collide with an obstacle) when the selection point of the robot 10 (e.g., the center of the body 11) occupies the space represented by the element 510D. For example, the robot 10 may be able to approach and travel parallel to a wall, but turning 90 degrees may result in a collision with the wall. In this example, some yaw configurations 512 (i.e., the yaw configurations 512 in which the robot 10 is parallel to the wall) are safe, while others (i.e., the yaw configurations 512 in which the robot 10 is perpendicular to the wall) are unsafe.

[0065] In some examples, the sparse map generator 410 determines an effective path through the yaw constraint region 510DZ (i.e., the region of the yaw constraint elements 510D) of the compressed configuration space map 500. Now refer to Figure 6, in some embodiments, the sparse graph generator 410 overlays the compressed configuration space map 500 with a plurality of Voronoi cells 610. The Voronoi cells 610 contain all the elements 510 in the yaw constraint region 510DZ and / or the yaw free region 510CZ that are equidistant from at least two elements 510 classified as yaw collision elements 510B. In other words, the Voronoi cells 610 are the elements 510 that are farthest from the yaw collision region 510BZ (and thus farthest from the body obstacles). Alternatively, the Voronoi cells 610 contain all the elements 510 in the yaw constraint region 510DZ and / or the yaw free region 510CZ that are equidistant from at least two body obstacle elements 510A. In Figure 6 the example, the cell 610 is located at the center of the yaw free region 510CZ and the yaw constraint region 510DZ between the yaw collision regions 510BZ. The Voronoi cells 610 are easy to calculate based on data that the robot 10 may have already accessed (e.g., the signed distance field).

[0066] To identify the elements 510 marked as Voronoi cells 610, the sparse graph generator 410 identifies the elements 510 (of the configuration space map 500) that are equidistant from the obstacles (i.e., the body obstacle elements 510A) on the configuration space map 500. That is, each element 510 inherently has a distance and a direction to the nearest obstacle boundary. Figure 7 depicts a portion of the configuration space map 500, containing two yaw collision regions 510BZ separated by the yaw constraint region 510DZ. Some Voronoi cells 610 cross the yaw constraint region 510DZ and are between the yaw collision regions 510BZ. In some examples, the sparse graph generator analyzes each element 510 of the configuration space map 500 to determine whether the element 510 should be marked as a Voronoi cell 610. When analyzing each element 510 to determine whether the element 510 is a Voronoi cell 610, in some embodiments, the sparse graph generator 410 determines whether the element 510 (i.e., the element 510 currently being analyzed) and the adjacent elements 510 point to substantially different locations as the nearest obstacle boundary. That is, for each element 510 in the configuration space map 500, the sparse graph generator 410 can locate the nearest obstacle boundary of the corresponding element, the nearest obstacle boundary adjacent to the bottom of the corresponding element, and the nearest obstacle boundary adjacent to the right side of the corresponding element. Whenever the distance between any two of the three nearest obstacle boundaries satisfies a threshold distance, the element 510 can be marked as a Voronoi cell 610. The threshold distance can be expressed as any number of elements (e.g., one element, two elements, five elements, etc.).

[0067] In the example shown, when the sparse graph generator 410 analyzes the element 510 labeled "A", the sparse graph generator 410 determines that the nearest yaw collision element 510B (or body obstacle element 510A) to the element "A" is the element 510a. The sparse graph generator 410 then determines the nearest yaw collision element 510B adjacent to the right side of the element (labeled "B"), which in this case is the element 510b. The sparse graph generator 410 can then determine the distance from the bottom adjacent (labeled "C") of the element to the nearest yaw collision element 510B (in this case, also the element 510a). In some examples, the sparse graph generator 410 can determine whether two of the three nearest obstacle boundaries (i.e., two of the three distances between "A" and "B", "A" and "C", and "B" and "C") are within a threshold distance of each other. When an element has two obstacle boundaries that are the same distance apart, the sparse graph generator 410 can determine the distance between the two elements.

[0068] In this example, the distance between the nearest obstacle boundaries of "A" and "B" is ten elements, the distance between the nearest obstacle boundaries of "A" and "C" is zero elements (i.e., they share the same nearest obstacle boundary), and the distance between the nearest obstacle boundaries of "B" and "C" is also ten elements. When the threshold distance is less than ten elements, the element "A" will be labeled as a Voronoi cell 610.

[0069] As previously discussed, the sparse graph generator 410 can perform a similar analysis for each element of the configuration space map 500. Continuing to refer to Figure 7 , the sparse graph generator 410 may not label the element "D" as a Voronoi cell 610 because the three nearest obstacle distances to its right adjacent ("E") and bottom adjacent ("F") do not meet the threshold (e.g., three elements). This is because the distance between the nearest obstacle boundaries (i.e., the elements 510c, 510d) is only one element, which may not meet the threshold.

[0070] Now referring to Figures 8A - 8E , in some embodiments, the sparse graph generator 410 determines the edges 810 and nodes 812 derived from the Voronoi cell 610. Figure 8AAn exemplary Voronoi cell 610 in the yaw constraint region 510DZ is shown. In some embodiments, the sparse graph generator selects any (i.e., random or near-random) element 510ff labeled as the Voronoi cell 610. Starting from the arbitrary element 510ff, the sparse graph generator performs a flood fill and records the distance from each element 510 labeled as the Voronoi cell 610 to the arbitrarily selected element 510ff. In the example shown, each element is labeled with the distance (in elements) from the arbitrary element 510ff. Flood fill is an algorithm for determining the region connected to a given node in an array. In this case, the flood fill continues through all elements 510 labeled as the Voronoi cell 610 until all elements have been assigned a distance. Although the arbitrarily selected element 510ff can be chosen completely randomly, it can be advantageous to restrict the selection to elements 510 near the robot 10 in order to minimize the effect from any errors.

[0071] After each element labeled as the Voronoi cell 610 has been assigned a distance, in some examples, the sparse graph generator 410 determines the ends or end nodes 812 of each branch of the Voronoi cell 610. The end node 812 of each branch will have a locally maximum distance from the arbitrarily selected element 510ff. In some examples, the sparse graph generator 410 starts labeling each element 510 from the end node 812 of the branch while traveling back towards the selected element 510ff. Whenever the sparse graph generator 410 encounters an element 510 that has already been labeled previously, the generator can attach a unique label to the combination of elements. For example, in Figure 8B the sparse graph generator 410 starts at the end node 812a and travels back towards the selected element 510ff, labeling each element along the way to form an edge 810a. That is, the Voronoi cell 610 from the end node 812a to the selected element 510ff can form an edge 810a. The sparse graph generator 410 can continue this process until all elements have been labeled. For example, in Figure 8C the sparse graph generator 410 travels from the end node 812b towards the element 510ff and labels each element to form another edge 810b until it encounters a previously labeled element at 812c ( Figure 8B ). The sparse graph generator 410 can assign a unique label to the junction element (i.e., the first previously labeled element encountered) at 812c. The sparse graph generator 410 can continue traveling back until it reaches the selected element 510ff, noting each element previously labeled as a separate edge 810c.

[0072] Now refer to Figure 8D, the sparse graph generator 410 can start from the end node 812d and travel back to the element 510ff, marking each element along the way again to form another edge 810d. After all elements 510 have been marked (and assigned to the edges 810), in some embodiments, the sparse graph generator 410 determines whether the selected element 510ff touches only two edges 810 (i.e., is not a connection point between three or more edges). In such a case, the sparse graph generator 410 can combine the two edges into a single edge. For example, as Figure 8D shown, the selected element 510ff touches only two other edges (i.e., edge 810c and edge 810d), and in Figure 8E , edge 810c and edge 810d are combined into a single edge 810. The final result of flood filling and element marking provides the sparse graph generator 410 with an ordered set of elements 510 along the edges 810 and a series of node elements 812. The series of nodes and edges form the sparse graph 416S and represent an effective path for the robot 10 to travel through the constrained space of the nearby environment (i.e., the yaw-constrained region 510DZ of the compressed configuration space map 500).

[0073] Figure 9 shows the configuration space map 500, which has Voronoi cells 610 classified as multiple edges 810 and nodes 812, with each edge 810 connected to another edge 810 (through a node 812) or to the yaw-free region 510CZ (also through a node 812). In some embodiments, when generating the sparse graph 416S, the sparse graph generator 410 only considers the elements 510 of the Voronoi cells 610 that are marked as being in the yaw-constrained region 510DZ, because paths through the yaw-free region 510DZ can be determined later. As referenced in Figures 8A - 8EAs discussed, the sparse graph generator 410 can divide elements marked as Voronoi cells 610 into individual edges 810 and nodes 812. In some examples, the sparse graph generator 410 marks or identifies nodes 812 where the edges 810 connect to the yaw-free zone 510CZ. That is, the sparse graph generator 410 identifies nodes 812 of the Voronoi cell 610 along the connection between the yaw-free zone 510CZ and the yaw-constrained zone 510DZ (i.e., the end or the connection point between the edge 810 or edges 810). The sparse graph generator 410 can also identify nodes 812 that are connection points of multiple edges 810. In some examples, the sparse graph generator 410 assumes that all nodes 812 connected to the same yaw-free zone 510CZ are connected to each other. That is, as long as there is a path between the nodes 812 (i.e., connected to the same yaw-free zone 510CZ), the sparse graph generator can (at least temporarily) ignore the movement through the yaw-free zone 510CZ. Having the edges 810 and nodes 812, the sparse graph generator 410 can first generate the final 2D configuration map 1000 by pruning the edges 810 that do not have navigation significance, based on the previously determined edges 810 and nodes 812 in the compressed configuration space map 500 that overlap with the Voronoi cell 610 ( Figure 10 ). For example, Figure 10 shows a compressed configuration space map 500 that is covered with multiple Voronoi cells 610 and has multiple insignificant edges 1010 that are pruned to generate the final 2D configuration map 1000. That is, in some embodiments, the sparse graph generator 410 prunes insignificant edges 1010 that are shorter than a threshold length and terminate or end within the yaw-constrained region 510DZ because these paths are highly unlikely to be worth traversing by the robot 10. Optionally, the threshold length can be approximately the radius of the robot 10. In some examples, when, after pruning, the connection point node 812 is connected to fewer than three edges 810, the sparse graph generator 410 combines the edges 810 together.

[0074] Now referring to Figure 11A and Figure 11B , in some embodiments, the sparse graph generator 410 converts the final 2D configuration map 1000 ( Figure 11A ) into an SE2 representation 1100 of the graph ( Figure 11B ). SE2 is a coordinate system that includes yaw (i.e., x coordinate, y coordinate, and yaw) with standard Cartesian coordinates. That is, the SE2 map 1100 takes the final 2D configuration map 1000 and includes yaw. Figure 11BShows SE2 figure 1100, where the yaw is the altitude. Although the robot 10 can have any number of yaw configurations 512 (e.g., 64 as discussed previously), SE2 figure 1100 can be simplified by considering only two yaw configurations - forward and backward. That is, generally, the robot 10 will travel in a yaw configuration 512 aligned with its travel path, and thus, SE2 figure 1100 can be greatly simplified by considering only these two yaw configurations. Each edge 810 of the 2D map 1000 can be represented by two SE2 edges with opposite tangents (i.e., "forward" and "backward" yaw configurations) 1110a, 1110b. Each 2D node 812 ( Figure 11A is represented by twice the number of edges 1110 ( Figure 11B connected to the node 812 in the 2D map 1000 representation. For example, Figure 11A shows three edges 810 connected at a single node 812, which translates to six nodes 1112 in the SE2 figure 1100. This means the robot 10 will rotate in place at the nodes 1112, and the sparse graph generator 410 can determine the interconnections between the nodes 1112 based on the valid yaw configurations 512 from the fully configured space map 414.

[0075] To finally determine the SE2 map 1100, in some embodiments, the sparse graph generator 410 determines an entry point and an exit point for the robot 10 from the map 1100. Now referring to Figure 12A and Figure 12B , the sparse graph generator 410 can project the off-map target 1210 (i.e., the target destination) onto the map 1100 boundary along a straight line. When the target 1210 is set in the yaw free zone 510CZ, the sparse graph generator 410 can use the corresponding element associated with the target 1210. However, when in the yaw constraint zone 510DZ, the sparse graph generator 410 can search along the nearest element 510 marked as a Voronoi cell 610 (from the 2D map 1000) or the boundary of the yaw free zone 510CZ. If the nearest element is on the boundary of the yaw free zone 510CZ, the sparse graph generator 410 can set this element as the target. When the nearest element is marked as a Voronoi cell 610 on the edge 810, the sparse graph generator 410 can bisect the edge 810 by placing a new node 1220 at the corresponding nearest element. In Figure 12AIn the example shown, the target 1210 is set in the yaw constraint zone 510DZ. The sparse graph generator determines that the nearest desired element is the Voronoi cell 610. The sparse graph generator 410 creates a new node 1220 at the element closest to the target 1210 and divides the previous edge 810 into two independent edges 810. In some embodiments, the sparse graph generator 410 creates two new nodes 1220, one node 1220 representing a forward yaw configuration 512 and one node 1220 representing a backward yaw configuration 512.

[0076] Once the new node 1220 has been placed, the sparse graph generator 410 determines whether the starting position of the robot 10 (e.g., the current position of the robot) is connected to the forward yaw configuration node 1220, the backward yaw configuration node 1220, or both. That is, the sparse graph generator 410 determines whether the robot 10 can connect to the node 1220 with the corresponding yaw configuration. In some examples, the sparse graph generator 410 determines the angle 1222 between the original or true target 1210 and the two yaw configurations (i.e., forward and backward) along the edge 810 of the new node 1220. In Figure 12B the example shown, when the angle meets a threshold (e.g., when the angle is within the region 1230), the forward-facing node 1220 is connected to the position of the robot. When the angle meets a second threshold (e.g., when the angle is within the region 1240), the backward-facing node 1220 is connected to the position of the robot. When the angle meets a third threshold (e.g., when the angle is within the region 1250), both the forward-facing node 1220 and the backward-facing node 1220 are connected to the starting position of the robot. That is, the robot 10 can travel either forward or backward to the position of the node 1220.

[0077] In some examples, the sparse graph generator 410 connects the nodes 812 indicating that the robot 10 will collide with an obstacle. For example, the sparse graph generator 410 can always assume that the robot turns by rotating from the center point, which may make some turns impossible. However, in some cases, the low-level path generator 130 can still navigate the path by opening pivot points that are not at the center of the robot 10, and in these cases, the sparse graph generator 410 can create an edge 810 between the nodes 812 that the generator 120 might otherwise determine the robot cannot cross.

[0078] Return reference Figure 4 , the intermediate path point generator 120 also includes a coarse path planner 420, which is configured to receive the sparse graph 416S from the sparse graph generator 410 and determine the path for the robot 10 to travel to reach the target 1210 (i.e., the node 1220 corresponding to the target destination ( Figure 12Aa topologically correct path (i.e., the coarse path 1310). In some embodiments, the coarse path planner 420 also receives the fully configured space map 414 and / or the compressed configured space map 500. Now refer to Figure 4 and Figure 13 , the map 1100 includes a coarse path 1310 that navigates from a starting point 1320 in the yaw-free zone 510CZ, through the yaw-constrained zone 510DZ to another yaw-free zone 510CZ.

[0079] To generate the path 1310, the coarse path planner 420 can implement a search algorithm (e.g., the A* search algorithm). The coarse path planner 420 can use the search algorithm to search a dense grid or graph 416D of the 2D yaw-free zone 510CZ to plot the path 1310. The sparse graph generator 410 can also generate the dense graph 416D, combine it with the sparse graph 416S, and send the combined graph to the coarse path planner 420. Alternatively, the coarse path planner can generate the dense graph 416D. The dense graph is a list of interconnected nodes of the yaw-free zone 510CZ determined according to the compressed configured space map 500. That is, just as the sparse graph 416S contains a list of interconnected nodes 812 of the elements 510 within the yaw-constrained zone 510DZ, the dense graph 416D contains a list of interconnected nodes of the elements 510 within the yaw-free zone 510DZ. Since the yaw-free zone does not require orientation (i.e., yaw) information (because the robot 10 can rotate freely in the yaw-free zone 510CZ), the coarse path planner 420 can search the dense graph 416D (denser than the sparse graph 416S). The coarse path planner 420 can combine the sparse graph 416S and the dense graph 416D to form a final graph 416F. The final graph 416F contains all the interconnected nodes within both the yaw-free zone 510CZ and the yaw-constrained zone 510DZ. The coarse path planner 420 can also use the search algorithm to search the final graph 416F to find the Voronoi cells 610 in the yaw-constrained zone 510DZ and the yaw-free zone 510CZ with the included orientation information. The coarse path planner 420 can link the yaw-free zone 510CZ with the yaw-constrained zone 510DZ and cross the yaw-constrained zone 510DZ with the coarse path 1310.

[0080] Because the dense map 416D lacks orientation information (and thus the final map 416F lacks orientation information in the yaw free zone 510CZ), in some embodiments, the coarse path planner 420 determines whether the orientation of any edge 810 (i.e., forward or backward) should be reversed. Optionally, the coarse path planner 420 is biased to walk forward along the edge 810. The coarse path planner 420 can identify segments of the path 1310 where the yaw direction can be reversed (i.e., both forward and backward directions are valid). The coarse path planner 420 can then calculate the cost of traversing the path 1310 (i.e., the amount of rotation required).

[0081] For example, now referring to Figure 14 map 1100, the coarse path planner 420 has planned a path 1430 from the start position 1410 (with an orientation aligned with the corresponding arrow) to the goal position 1420 (with a direction aligned with the corresponding arrow). Due to the forward bias, the search algorithm can select the forward-facing orientation (path A-C) through the yaw constraint zone 510DZ. The coarse path planner 420 optionally calculates the cost of path A-C (forward-facing) and path B-D (backward-facing). In the given example, since the given start and goal orientations face backward, the backward path B-D is cheaper (i.e., requires less rotation) than the forward-facing path A-C, and the coarse path planner 420 can update the path 1430 accordingly.

[0082] Returning to the reference Figure 4 , in some embodiments, the intermediate path point generator 120 also includes a path point placer 430. The path point placer 430 receives the fully configured space map 414 and the sparse map 416S with the path 1310 from the coarse path planner 420. The path point placer 430 processes the path 1310 to find suitable locations to place the intermediate path points 310. In some examples, the path point placer 430 searches along the path 1310 until the line of sight is first lost (e.g., due to an obstacle) and places the path point 310 at that location. Optionally, the path point placer 430 downsamples the path 1310 before the search to reduce the processing time.

[0083] Now referring to Figure 15A, the robot 10 navigates along the path 1310 through the yaw constraint zone 510DZ located between two body obstacle zones 510AZ. The path point placer 430 attracts the rays 1510 from the current position of the robot 10 to various points along the path 1310. When one of the rays 1510 intersects a part of the body obstacle zone 510 (or, alternatively, within a threshold distance of the body obstacle zone 510 to account for the width of the robot 10), the path point placer 430 places the intermediate path point 310 on the path 1310 at the point 1530 where the ray 1510 intersects the path 1310 or just before the line of sight is lost. Thus, it is relatively simple to determine the position where the line of sight is lost in Cartesian coordinates (i.e., x and y coordinates). However, determining the line of sight in SE2 space (i.e., including yaw) is more complex.

[0084] Reference Figure 15B , to address the included yaw, the path point placer 430 can determine the minimum allowable yaw and the maximum allowable yaw at each point along the downsampled path 1310 to generate envelopes 1550, 1550a–n. In some examples, the path point placer 430 tracks the minimum (i.e., innermost) envelopes 1560, 1560a–n at each downsampled point, and places the intermediate path point 310 when the envelope width drops below the envelope threshold or when the required yaw configuration at the point 1530 on the path 1310 is outside either of the envelopes 1550. In the example shown, the yaw of the envelope 1550c is outside the inner envelope 1560c and is a candidate for the intermediate path point 310.

[0085] In some examples, if the azimuth changes by more than a threshold amount (e.g., 60 degrees), the path point placer 430 also adds an intermediate path point 310 when moving from the yaw-free zone 510CZ to the yaw constraint zone 510DZ. Optionally, when the azimuth at the target (i.e., the target pose) deviates from the current map (i.e., is too far from the current position of the robot) and the line of sight to the target is lost, the path point placer 430 can add an intermediate path point 310 at the end of the path 1310. Additionally, the path point placer 430 can move the path point away from the connection point in a narrow space. For example, the intermediate path point 310 can be projected down onto the path 1310 until outside the threshold radius surrounding the connection point.

[0086] The high-level waypoint 210 can include two coordinates (i.e., x and y coordinates) indicating a position on a plane, a yaw value, and a time value. The time value can indicate an estimated amount of time for the robot to navigate to the corresponding waypoint 310. In addition to the x, y, and yaw values, the waypoint placer 430 can similarly add the estimated time value to the intermediate waypoint 310. The estimated time value can be determined by considering the maximum amount of time from the starting position to the waypoint in x, y and the yaw using any received speed limit. The waypoint placer 430 can time-offset for each high-level waypoint 210 on the planned trajectory after the intermediate waypoint 310.

[0087] Return reference Figure 4 , the waypoint placer 430 adds any intermediate waypoints 310 to the high-level waypoints 210 of the navigation route 112. The intermediate waypoint generator passes the navigation route 112 to the low-level path generator 130 to generate a footstep plan 142 for navigating to each waypoint 210, 310. In the case where the intermediate waypoint generator 120 determines that the intermediate waypoint 310 is not necessary (or fails to find a suitable location), the intermediate waypoint generator 120 can pass the received navigation route 112 to the low-level path generator 130 unchanged.

[0088] Figure 16 is a flowchart of an exemplary operational arrangement of a method 1600 for generating intermediate waypoints 310 for a robot 10. At operation 1602, the method 1600 includes receiving a navigation route 112 at the data processing hardware 36 of the robot 10. The navigation route 112 includes a series of high-level waypoints 210 that begin at a starting position 113 and end at a destination position 114. The navigation route 112 is based on high-level navigation data 50 that represents the positions of static obstacles in the area that the robot 10 is to navigate through.

[0089] The method 1600 includes, at operation 1604, receiving image data 17 of the environment 8 around the robot 10 from an image sensor 31 at the data processing hardware 36. At operation 1606, the method 1600 includes generating, by the data processing hardware 36, at least one intermediate waypoint 310 based on the image data 17, and at operation 1608, adding, by the data processing hardware 36, the at least one intermediate waypoint 310 to the series of high-level waypoints 210 of the navigation route 112. At operation 1610, the method 1600 includes navigating, by the data processing hardware 36, the robot 10 from the starting position 113 along the series of high-level waypoints 210 and the at least one intermediate waypoint 310 toward the destination position 114.

[0090] Figure 17It is a schematic diagram of an example computing device 1700 (e.g., data processing hardware 36 and memory hardware 20) that can be used to implement the systems and methods described in this document. The components shown here, their connections and relationships, and their functions are merely exemplary and are not meant to limit the implementation of the inventions described and / or claimed in this document.

[0091] The computing device 1700 includes a processor 1710 (e.g., data processing hardware 36), a memory 1720 (e.g., memory hardware 38), a storage device 1730, a high-speed interface / controller 1740 connected to the memory 1720 and a high-speed expansion port 1750, and a low-speed interface / controller 1760 connected to a low-speed bus 1770 and the storage device 1730. Each of the components 1710, 1720, 1730, 1740, 1750, and 1760 is interconnected using various buses and can be mounted on a common motherboard or in other suitable ways. The processor 1710 (e.g., data processing hardware) can process instructions for execution within the computing device 1700, including instructions stored in the memory 1720 or on the storage device 1730, to display graphical information of a graphical user interface (GUI) on an external input / output device, such as a display 1780 coupled to the high-speed interface 1740. In other embodiments, multiple processors and / or multiple buses, as well as multiple memories and multiple types of memories, can be used as appropriate. Additionally, multiple computing devices 1700 can be connected, each providing a portion of the necessary operations (e.g., as a server group, a blade server group, or a multiprocessor system). The processor 1710 can execute Figure 1 and 4 the intermediate waypoint generator 120.

[0092] The memory 1720 stores information non-temporarily within the computing device 1700. The memory 1720 can be a computer-readable medium, a volatile storage unit, or a non-volatile storage unit. The non-transitory memory 1720 can be a physical device for temporarily or permanently storing programs (e.g., sequences of instructions) or data (e.g., program state information) for use by the computing device 1700. Examples of non-volatile memory include, but are not limited to, flash memory and read-only memory (ROM) / programmable read-only memory (PROM) / erasable programmable read-only memory (EPROM) / electrically erasable programmable read-only memory (EEPROM) (e.g., commonly used for firmware, such as a boot program). Examples of volatile memory include, but are not limited to, random access memory (RAM), dynamic random access memory (DRAM), static random access memory (SRAM), phase change memory (PCM), and magnetic disks or tapes.

[0093] The storage device 1730 can provide large - scale storage for the computing device 1700. In some embodiments, the storage device 1730 is a computer - readable medium. In various different embodiments, the storage device 1730 can be a floppy disk device, a hard disk device, an optical disk device, or a magnetic tape device, a flash memory, or other similar solid - state memory devices, or an array of devices, including devices in a storage area network or other configurations. In additional embodiments, a computer program product is tangibly embodied in an information carrier. The computer program product contains instructions that, when executed, perform one or more methods, such as those described above. The information carrier is a computer - readable or machine - readable medium, such as the memory 1720, the storage device 1730, or the memory on the processor 1710.

[0094] The high - speed controller 1740 manages the bandwidth - intensive operations of the computing device 1700, while the low - speed controller 1760 manages the lower - bandwidth - intensive operations. Such a division of responsibilities is merely exemplary. In some embodiments, the high - speed controller 1740 is coupled to the memory 1720 and the high - speed expansion port 1750, and the high - speed expansion port 1750 can accept various expansion cards (not shown). In some embodiments, the low - speed controller 1760 is coupled to the storage device 1730 and the low - speed expansion port 1790. The low - speed expansion port 1790, which can include various communication ports (e.g., Universal Serial Bus, Bluetooth, Ethernet, Wireless Ethernet), can be coupled to one or more input / output devices, such as a keyboard, a pointing device, a scanner, or a network device, such as a switch or a router, for example, through a network adapter.

[0095] Various embodiments of the systems and techniques described herein can be implemented in digital electronic and / or optical circuits, integrated circuits, application - specific integrated circuits (ASICs) specifically designed, computer hardware, firmware, software, and / or combinations thereof. These various embodiments can include embodiments in one or more computer programs executable and / or interpretable on a programmable system that includes at least one programmable processor, which can be special - purpose or general - purpose, coupled to receive data and instructions from a storage system, at least one input device, and at least one output device, and to send data and instructions to the storage system, the at least one input device, and the at least one output device.

[0096] These computer programs (also referred to as programs, software, software applications, or code) include machine instructions for a programmable processor and can be implemented in high-level procedural and / or object-oriented programming languages and / or assembly / machine languages. As used herein, the terms “machine-readable medium” and “computer-readable medium” refer to any computer program product, non-transitory computer-readable medium, apparatus, and / or device (e.g., a magnetic disk, an optical disk, a memory, a programmable logic device (PLD)) that provides machine instructions and / or data to a programmable processor, including a machine-readable medium that receives machine instructions as a machine-readable signal. The term “machine-readable signal” refers to any signal that provides machine instructions and / or data to a programmable processor.

[0097] The processes and logical flows described in this specification can be performed by one or more programmable processors (also referred to as data processing hardware) that execute one or more computer programs to perform functions by operating on input data and generating output. These processes and logical flows can also be performed by special purpose logic circuitry, such as a field programmable gate array (FPGA) or an application specific integrated circuit (ASIC). By way of example, a processor suitable for the execution of a computer program includes both general and special purpose microprocessors, as well as any one or more processors of any type of digital computer. Generally, a processor will receive instructions and data from a read only memory or a random access memory or both. The essential elements of a computer are a processor for executing instructions and one or more memory devices for storing instructions and data. Generally, a computer will also include, or be operatively coupled to receive data from or transfer data to, one or more mass storage devices (such as magnetic disks, magneto-optical disks, or optical disks) for storing data. However, a computer need not have such devices. Computer-readable media suitable for storing computer program instructions and data include all forms of non-volatile memory, media, and storage devices, including, by way of example, semiconductor memory devices, such as EPROM, EEPROM, and flash memory devices; magnetic disks, such as internal hard disks or removable disks; magneto-optical disks; and CD-ROM and DVD-ROM disks. The processor and the memory can be supplemented by, or incorporated in, special purpose logic circuitry.

[0098] Multiple embodiments have been described. However, it should be understood that various modifications can be made without departing from the spirit and scope of the present disclosure. Accordingly, other embodiments are within the scope of the following claims.

Claims

1. A computer-implemented method, comprising: Receiving a navigation route at a data processing hardware of a robot, the navigation route including a first waypoint and a second waypoint; Receiving, at the data processing hardware, image data associated with an environment of the robot from an image sensor; Determining, by the data processing hardware, multiple paths from the first waypoint to the second waypoint based on the image data; Selecting, by the data processing hardware, a path from the multiple paths, the path including at least one intermediate waypoint; And Commanding, by the data processing hardware, the robot to navigate from a starting position to a destination position in the environment via the first waypoint, the at least one intermediate waypoint, and the second waypoint.

2. The computer-implemented method according to claim 1, wherein the navigation route is based on one or more first obstacles in the environment, and wherein the multiple paths are based on one or more second obstacles in the environment.

3. The computer-implemented method according to claim 1, wherein the navigation route is based on one or more first positions of one or more obstacles in the environment, and wherein the image data indicates one or more second positions of the one or more obstacles.

4. The computer-implemented method according to claim 1, wherein, The multiple paths include multiple paths that bypass obstacles.

5. The computer-implemented method according to claim 1, wherein, The navigation route includes a navigation route from the starting position to the destination position.

6. The computer-implemented method according to claim 1, further comprising: Generating a footstep plan indicating one or more positions for placing one or more feet of the robot based on the first waypoint, the at least one intermediate waypoint, and the second waypoint.

7. The computer-implemented method according to claim 6, wherein commanding the navigation of the robot includes: Using the footstep plan to command the robot to navigate from the starting position to the destination position via the first waypoint, the at least one intermediate waypoint, and the second waypoint.

8. The computer-implemented method according to claim 6, wherein the at least one intermediate waypoint includes an intermediate waypoint, and wherein commanding the navigation of the robot includes: Using the footstep plan to command the robot to navigate from the starting position to the first waypoint using the footstep plan; Using the footstep plan to command the robot to navigate from the first waypoint to the intermediate waypoint; And Using the footstep plan to command the robot to navigate from the intermediate waypoint to the second waypoint.

9. The computer-implemented method according to claim 6, wherein commanding the navigation of the robot includes: Commanding the placement of one or more feet of the robot according to the footstep plan.

10. The computer-implemented method according to claim 1, further comprising: Adding the at least one intermediate waypoint to the navigation route.

11. The computer-implemented method according to claim 1, wherein receiving the image data includes: Receiving the image data based on navigation of the at least a portion of the environment by the robot along at least a portion of the navigation route.

12. The computer-implemented method according to claim 1, further comprising: Comparing a first path among the plurality of paths with a second path among the plurality of paths, wherein the selected path is based on comparing the first path with the second path.

13. The computer-implemented method according to claim 12, further comprising: Determining a first cost associated with the first path; and Determining a second cost associated with the second path, wherein comparing the first path with the second path includes comparing the first cost with the second cost.

14. The computer-implemented method according to claim 1, wherein determining the plurality of paths includes: Determining the plurality of paths based on the image data to avoid the robot contacting the one or more obstacles.

15. The computer-implemented method according to claim 1, wherein determining the plurality of paths includes: Determining the plurality of paths based on the navigation route.

16. The computer-implemented method according to claim 1, further comprising: Generating the plurality of paths based on one or more obstacles indicated by the image data.

17. The computer-implemented method according to claim 1, further comprising: Determining a location for placing the at least one intermediate path point based on the image data.

18. The computer-implemented method according to claim 1, wherein a first set of data is used to generate the first path point and the second path point, wherein a second set of data is used to generate the at least one intermediate path point, and wherein the first set of data is different from the second set of data.

19. The computer-implemented method according to claim 1, wherein, Each of the first path point, the second path point, and the at least one intermediate path point is associated with at least one of a corresponding location on the ground, a corresponding yaw value, or a corresponding time value.

20. The computer-implemented method according to claim 1, wherein, Each of the plurality of paths deviates from the navigation route.

21. A system, comprising: Data processing hardware; and A memory in communication with the data processing hardware, the memory storing instructions, wherein the instructions, when executed by the data processing hardware, cause the data processing hardware to: Receive a navigation route, the navigation route including a first path point and a second path point; Receive image data associated with the environment of a robot from an image sensor; Determine a plurality of paths from the first path point to the second path point based on the image data; Select a path from the plurality of paths, the path including at least one intermediate path point; And Command the robot to navigate from a starting position to a destination position in the environment via the first path point, the at least one intermediate path point, and the second path point.

22. A robot, comprising: An image sensor; At least two legs; Data processing hardware in communication with the image sensor; and A memory communicating with the data processing hardware, the memory storing instructions, wherein execution of the instructions by the data processing hardware causes the data processing hardware to: Receive a navigation route, the navigation route including a first waypoint and a second waypoint; Receive image data associated with the environment of the robot from the image sensor; Determine, based on the image data, multiple paths from the first waypoint to the second waypoint; Select a path from the multiple paths, the path including at least one intermediate waypoint; And Command the robot to navigate from a starting position to a destination position in the environment via the first waypoint, the at least one intermediate waypoint, and the second waypoint.

23. A computer-implemented method that, when executed by data processing hardware of a robot, causes the data processing hardware to perform operations including: Receive image data of the environment of the robot from an image sensor; Receive a body obstacle map, the body obstacle map including a map of obstacles that the body of the robot cannot cross; Generate a sparse graph based on the body obstacle map, the sparse graph including a set of nodes and edges starting from a starting position and ending at a destination, the nodes and edges representing paths along which the robot can travel in the environment; Plan a rough path from the starting position to the destination; Determine a point along the rough path between a first node and a second node in the set of nodes where the line of sight of the image sensor is lost; Generate an intermediate waypoint at the point along the rough path where the line of sight of the image sensor is lost; And Navigate the robot from the starting position towards the destination along the rough path and the intermediate waypoint.

24. The method according to claim 23, wherein said planning of the rough path comprises: Determine a route of effective interconnection between corresponding nodes and edges in the set of nodes and edges.

25. The method according to claim 23, wherein generating the sparse graph includes: Generating a fully configured space map based on the body obstacle map, the fully configured space map including a two-dimensional grid of elements, each element of the grid representing a space in the environment, and each element of the fully configured space map including a corresponding set of yaw configurations; and Generating the sparse graph from the fully configured space map.

26. The method according to claim 25, wherein: Each yaw configuration is classified as valid or invalid; A valid yaw configuration represents a yaw configuration of the robot that is free from the risk of contacting an obstacle at a space associated with the corresponding element; and An invalid yaw configuration represents a yaw configuration of the robot that is not free from the risk of contacting an obstacle at a space associated with the corresponding element.

27. The method according to claim 25, wherein generating the sparse graph includes: Generating a compressed configured space map from the fully configured space map, the compressed configured space map including a second two-dimensional grid of elements, each element of the second grid representing a space in the environment, and each element of the second grid being classified as one of the following: A yaw collision zone; A yaw free zone, or A yaw constraint zone; and Generate the sparse graph from the compressed configuration space map.

28. The method according to claim 27, wherein planning a rough path from a starting position to a destination includes: Generating a dense graph from the compressed configuration space map, the dense graph including elements classified as yaw-free zones; and Linking edges from the sparse graph with elements from the dense graph.

29. The method according to claim 28, wherein linking the edges with elements from the dense graph includes: Combining the sparse graph and the dense graph to generate a final graph; and Performing an A* search algorithm on the final graph.

30. The method according to claim 23, wherein determining points along the rough path at which the line of sight of the image sensor is lost includes: Determining a minimum allowable yaw and a maximum allowable yaw at each element along the planned rough path; Determining a minimum envelope based on the minimum allowable yaw and the maximum allowable yaw; and and Determining that the required yaw at points along the rough path is outside the minimum envelope.

31. The method according to claim 23, wherein the destination represents the position of the robot in the environment.

32. The method according to claim 23, wherein the destination includes a corresponding node in the set of nodes of the sparse graph.

33. A robot, comprising: A body; Legs coupled to the body and configured to maneuver the robot in an environment; Data processing hardware in communication with the legs; and Memory hardware in communication with the data processing hardware, the memory hardware storing instructions that, when executed on the data processing hardware, cause the data processing hardware to perform operations including: Receiving image data of the environment of the robot from an image sensor; Receiving a body obstacle map including a map of obstacles that the body of the robot cannot cross; Generating a sparse graph based on the body obstacle map, the sparse graph including a set of nodes and edges starting from a starting position and ending at a destination, the nodes and edges representing paths along which the robot can travel in the environment; Planning a rough path from the starting position to the destination; Determining points along the rough path between a first node in the set of nodes and a second node in the set of nodes at which the line of sight of the image sensor is lost; Generating intermediate path points at the points along the rough path at which the line of sight of the image sensor is lost; and Navigating the robot from the starting position towards the destination along the rough path and the intermediate path points.

34. The robot according to claim 33, wherein the planning of the rough path includes: Determining a route for effective interconnection between corresponding nodes and edges in the set of nodes and edges.

35. The robot according to claim 33, wherein generating the sparse graph includes: Generating a full configuration space map based on the body obstacle map, the full configuration space map including a two-dimensional grid of elements, each element of the grid representing a space in the environment, and each element of the full configuration space map including a corresponding set of yaw configurations; and Generating the sparse graph from the full configuration space map.

36. The robot according to claim 35, wherein: Each yaw configuration is classified as valid or invalid; A valid yaw configuration represents a yaw configuration of the robot that is free from the risk of contacting an obstacle at a space associated with a corresponding element; and An invalid yaw configuration represents a yaw configuration of the robot that is not free from the risk of contacting an obstacle at a space associated with a corresponding element.

37. The robot according to claim 35, wherein generating the sparse map includes: Generating a compressed configuration space map from the full configuration space map, the compressed configuration space map including a second two-dimensional grid of elements, each element of the second grid representing a space of the environment, and each element of the second grid being classified as one of the following: A yaw collision zone; A yaw free zone, or A yaw constraint zone; and Generating the sparse map from the compressed configuration space map.

38. The robot according to claim 37, wherein planning the rough path from the starting position to the destination includes: Generating a dense map from the compressed configuration space map, the dense map including elements classified as yaw free zones; and Linking the edges from the sparse map with the elements from the dense map.

39. The robot according to claim 38, wherein linking the edges with the elements from the dense map includes: Combining the sparse map and the dense map to generate a final map; and Performing an A* search algorithm on the final map.

40. The robot according to claim 33, wherein, Determining a point along the rough path at which the line of sight of the image sensor is lost includes: Determining a minimum allowable yaw and a maximum allowable yaw at each element along the planned rough path; Determining a minimum envelope based on the minimum allowable yaw and the maximum allowable yaw; and Determining that the required yaw at a point on the rough path is outside the minimum envelope.

41. The robot according to claim 33, wherein the destination represents a position of the robot in the environment.

42. The robot according to claim 33, wherein the destination includes a corresponding node in the set of nodes of the sparse map.

43. A method includes: Receiving, at data processing hardware of a robot, a navigation route that includes a series of high-level waypoints starting at a starting position and ending at a destination position, and the navigation route is based on high-level navigation data representing positions of static obstacles in an area in which the robot is to navigate; Receiving, at the data processing hardware, image data of an environment around the robot from an image sensor; Generating, by the data processing hardware, at least one intermediate waypoint based on the image data; Adding, by the data processing hardware, the at least one intermediate waypoint to the series of high-level waypoints of the navigation route; and Navigating, by the data processing hardware, the robot along the series of high-level waypoints and the at least one intermediate waypoint from the starting position toward the destination position, Each of the high-level waypoints and each of the at least one intermediate waypoint includes two coordinates indicating a position on a plane, a yaw value, and a time value, the time value indicating an estimated amount of time for the robot to navigate to the corresponding waypoint.

44. The method according to claim 43, further comprising maintaining, by the data processing hardware, each of the series of high-level waypoints on the navigation route.

45. The method according to claim 43, further comprising: Receiving, at the data processing hardware, a body obstacle map, the body obstacle map including a map of obstacles that the body of the robot cannot cross, wherein generating the at least one intermediate waypoint includes: Generating a sparse map based on the body obstacle map, the sparse map including a list of nodes and edges, the nodes and edges representing paths that the robot can travel in the environment; Planning a rough path from a first node in the list of nodes to a second node in the list of nodes, each of the first node and the second node representing a space in the environment; Determining a point along the rough path at which the line of sight of the image sensor is lost; and Generating one of the at least one intermediate waypoint at the point where the line of sight is lost.

46. The method according to claim 45, wherein generating the sparse map includes: Generating a complete configuration space map based on the body obstacle map, the complete configuration space map including a two-dimensional grid of elements, each element of the grid representing a space in the environment, and each element of the complete configuration space map including a corresponding set of yaw configurations, each yaw configuration being classified as valid or invalid, where a valid yaw configuration represents a yaw configuration of the robot that is free from the risk of contacting an obstacle at the space associated with the corresponding element, and an invalid yaw configuration represents a yaw configuration of the robot that is not free from the risk of contacting an obstacle at the space associated with the corresponding element; Generating a compressed configuration space map from the complete configuration space map, the compressed configuration space map including a second two-dimensional grid of elements, and each element of the second grid representing a space in the environment, and each element of the second grid being classified as one of: (i) a yaw collision zone, (ii) a yaw free zone, or (iii) a yaw constrained zone; and Generating the sparse map from the compressed configuration space map.

47. The method according to claim 46, wherein planning a rough path from the first node to the second node includes: Generating a dense map from the compressed configuration space map, the dense map including elements classified as yaw free zones; and Linking the edges from the sparse map with the elements from the dense map.

48. The method according to claim 47, wherein linking the edges with the elements from the dense map includes: Combining the sparse map and the dense map to generate a final map; and Performing an A* search algorithm on the final map.

49. The method according to claim 46, wherein generating the sparse map further includes: Overlay multiple Voronoi cells on the compressed configuration space map, where each Voronoi cell is classified as a yaw constraint region on the sparse map, and each Voronoi cell is equidistant from at least two elements classified as yaw collision regions.

50. The method according to claim 49, wherein generating the sparse map further comprises: Classifying each Voronoi cell as either an edge or a node.

51. The method according to claim 50, wherein classifying each Voronoi cell comprises: Performing a flood fill algorithm.

52. The method according to claim 46, wherein planning a rough path from the first node to the second node comprises: Trimming edges, where each trimmed edge comprises elements below a threshold length and classified as either a yaw collision region or a yaw constraint region.

53. The method according to claim 45, wherein determining points along the rough path where the line of sight of the image sensor is lost comprises: Determining a minimum allowable yaw and a maximum allowable yaw at each element along the planned rough path; Determining a minimum envelope based on the minimum allowable yaw and the maximum allowable yaw; and Determining that the required yaw at points on the rough path is outside the minimum envelope.

54. The method according to claim 43, further comprising sending, by the data processing hardware, a navigation route having the high-level path points and the at least one intermediate path point to a low-level path generator.

55. A method, comprising: Receiving, at a data processing hardware of a robot, a navigation route that includes a series of high-level path points starting at a starting position and ending at a destination position, and the navigation route is based on high-level navigation data representing the positions of static obstacles in the area in which the robot is to navigate; Receiving, at the data processing hardware, image data of the environment around the robot from an image sensor; Generating, by the data processing hardware, at least one intermediate path point based on the image data; Adding, by the data processing hardware, the at least one intermediate path point to the series of high-level path points of the navigation route; Navigating, by the data processing hardware, the robot from the starting position toward the destination position along the series of high-level path points and the at least one intermediate path point; Sending, by the data processing hardware, a navigation route having the high-level path points and the at least one intermediate path point to a low-level path generator; Determining, by the data processing hardware, whether to add the at least one intermediate path point to the navigation route; and In response to determining not to add the at least one intermediate path point to the navigation route, passing, by the data processing hardware, the unchanged navigation route to the low-level path generator.

56. A robot, comprising: A body; Legs coupled to the body and configured to maneuver the robot in an environment; Data processing hardware in communication with the legs; and Memory hardware communicating with the data processing hardware, the memory hardware storing instructions which, when executed on the data processing hardware, cause the data processing hardware to perform operations including: Receiving a navigation route, the navigation route including a series of high-level waypoints starting at a start position and ending at a destination position, and the navigation route being based on high-level navigation data representing the positions of static obstacles in the area the robot is to navigate; Receiving image data of the environment around the robot from an image sensor; Generating at least one intermediate waypoint based on the image data; Adding the at least one intermediate waypoint to the series of high-level waypoints of the navigation route; and Navigating the robot from the start position towards the destination position along the series of high-level waypoints and the at least one intermediate waypoint, wherein each high-level waypoint and each intermediate waypoint of the at least one intermediate waypoint includes: two coordinates indicating a position on a plane, a yaw value, and a time value, the time value indicating an estimated amount of time for the robot to navigate to the corresponding waypoint.

57. The robot according to claim 56, further comprising maintaining each of the series of high-level waypoints on the navigation route.

58. The robot according to claim 56, further comprising: Receiving a body obstacle map, the body obstacle map including a map of obstacles that the body of the robot cannot cross, wherein the generating of at least one intermediate waypoint includes: Generating a sparse map based on the body obstacle map, the sparse map including a list of nodes and edges, the nodes and edges representing paths that the robot can travel in the environment; Planning a rough path from a first node in the list of nodes to a second node in the list of nodes, each of the first node and the second node representing a space in the environment; Determining a point along the rough path where the line of sight of the image sensor is lost; and Generating one of the at least one intermediate waypoints at the point where the line of sight is lost.

59. The robot according to claim 58, wherein the generating of the sparse map includes: Generating a complete configuration space map based on the body obstacle map, the complete configuration space map including a two-dimensional grid of elements, each element of the grid representing a space in the environment, and each element of the complete configuration space map including a corresponding set of yaw configurations, each yaw configuration being classified as valid or invalid, where a valid yaw configuration represents a yaw configuration of the robot that is free from the risk of contacting an obstacle at the space associated with the corresponding element, and an invalid yaw configuration represents a yaw configuration of the robot that is not free from the risk of contacting an obstacle at the space associated with the corresponding element; Generate a compressed configuration space map from the full configuration space map, the compressed configuration space map including a second two-dimensional grid of elements, each element of the second grid representing a space of the environment, and each element of the second grid being classified as one of: (i) a yaw collision zone, (ii) a yaw free zone, or (iii) a yaw constraint zone; and Generate the sparse map from the compressed configuration space map.

60. The robot according to claim 59, wherein planning a rough path from the first node to the second node includes: Generate a dense map from the compressed configuration space map, the dense map including elements classified as yaw free zones; and Link edges from the sparse map with elements from the dense map.

61. The robot according to claim 60, wherein linking the edges with elements from the dense map includes: Combine the sparse map and the dense map to generate a final map; and Perform an A* search algorithm on the final map.

62. The robot according to claim 59, wherein generating the sparse map further includes: Overlay a plurality of Voronoi cells on the compressed configuration space map, each Voronoi cell being classified as a yaw constraint zone on the sparse map, and each Voronoi cell being equidistant from at least two elements classified as yaw collision zones.

63. The robot according to claim 62, wherein generating the sparse map further includes: Classify each Voronoi cell as either an edge or a node.

64. The robot according to claim 63, wherein classifying each Voronoi cell includes: Perform a flood fill algorithm.

65. The robot according to claim 59, wherein planning a rough path from the first node to the second node includes: Prune edges, each pruned edge including elements below a threshold length and classified as either a yaw collision zone or a yaw constraint zone.

66. The robot according to claim 58, wherein, Determining points along the rough path where the line of sight of the image sensor is lost includes: Determine a minimum allowable yaw and a maximum allowable yaw at each element along the planned rough path; Based on the minimum allowable yaw and the maximum allowable yaw, determine a minimum envelope; and and Determine that the required yaw at points on the rough path is outside the minimum envelope.

67. The robot according to claim 56, further comprising sending a navigation route having the high-level path points and the at least one intermediate path point to a low-level path generator.

68. A robot, comprising: A body; Legs, which are coupled to the body and are configured to maneuver the robot in an environment; Data processing hardware in communication with the legs; and Memory hardware in communication with the data processing hardware, the memory hardware storing instructions which, when executed on the data processing hardware, cause the data processing hardware to perform operations including: Receive a navigation route, the navigation route including a series of high-level waypoints starting at a starting position and ending at a destination position, and the navigation route being based on high-level navigation data representing the positions of static obstacles in the area in which the robot is to navigate; Receive image data of the environment around the robot from an image sensor; Generate at least one intermediate waypoint based on the image data; Add the at least one intermediate waypoint to the series of high-level waypoints of the navigation route; Navigate the robot from the starting position towards the destination position along the series of high-level waypoints and the at least one intermediate waypoint; Determine whether to add the at least one intermediate waypoint to the navigation route; and In response to determining not to add the at least one intermediate waypoint to the navigation route, pass the unchanged navigation route to a low-level path generator.

Images