Intermediate Waypoint Generator

Abstract

A method for generating intermediate waypoints (310) for a navigation system (100) of a robot (10), the method comprising receiving a navigation route (112). The navigation route comprises a series of high-level waypoints (210) starting at a starting location (113) and ending at a destination location (114), and is based on high-level navigation data (50) representing the locations of static obstacles in an area to be navigated by the robot. The method further comprises receiving image data (17) of an environment (8) surrounding the robot from an image sensor (31), and generating at least one intermediate waypoint based on the image data. The method further comprises 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 location along the series of high-level waypoints and the at least one intermediate waypoint towards the destination location.

Description

Technical Field

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

[0002] 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 making contact with obstacles or becoming stuck or trapped. As these robotic devices become more prevalent, real-time navigation and route planning that avoids contact with obstacles while successfully navigating to a destination is needed. Summary of the invention

[0003] 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 data processing hardware of the robot. The navigation route includes a series of high-level waypoints, the series of high-level waypoints starting at a starting position and ending at a destination position, and is based on high-level navigation data. The high-level navigation data represents the location of static obstacles in the area to be navigated by the robot. The method also includes receiving image data of the environment around the robot from an image sensor at the data processing hardware. The method also includes generating at least one intermediate waypoint based on the image data by the data processing hardware. The method also includes adding the at least one intermediate waypoint to the series of high-level waypoints of the navigation route by the data processing hardware, and navigating the robot from the starting position along the series of high-level waypoints and the at least one intermediate waypoint toward the destination position by the data processing hardware.

[0004] 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 each high-level path point in the series of high-level path points on the navigation route by data processing hardware. The method may also include receiving a body obstacle map at the data processing hardware. The body obstacle map includes a map of obstacles that the robot's body cannot traverse. Additionally, generating at least one intermediate path point may include generating a sparse graph based on the body obstacle map. The sparse graph includes a series of nodes and edges. The nodes and edges represent a path that the robot can travel in the environment. The method 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 method may also include determining a point where the line of sight of the image sensor along the rough path is lost, and generating one of the at least one intermediate path points at the point where the line of sight is lost.

[0005] In some embodiments, generating the sparse graph includes generating a fully configured spatial map based on the body obstacle map. The fully configured spatial map includes a two-dimensional grid of elements, wherein each element of the grid represents the space of the environment and each element of the fully configured spatial map includes a corresponding set of yaw configurations. Each yaw configuration can be classified as valid or invalid, wherein a valid yaw configuration represents a yaw configuration of a robot that is free from the danger of contacting obstacles at the space associated with the corresponding element, and an invalid yaw configuration represents a yaw configuration of a robot that cannot be free from the danger of contacting obstacles at the space associated with the corresponding element. The method may also include generating a compressed configuration spatial map from the fully configured spatial map. The compressed configuration spatial map includes a second two-dimensional grid of elements, and each element of the second grid represents the space of the environment. Each element of the second grid is also classified as one of: (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 graph from a compressed configuration spatial map.

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

[0007] In some examples, generating a sparse graph further includes overlaying a plurality of Voronoi cells on the compressed configuration space map, wherein 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 a sparse graph further includes classifying each Voronoi cell as either an edge or a node. Classifying each Voronoi cell may include executing a flood fill algorithm.

[0008] In some embodiments, planning a coarse path from the first node to the second node includes trimmed edges, and each trimmed edge includes an element that is below a threshold length and is classified as either a yaw collision zone or a yaw constraint zone. Determining a point along the coarse path at which a line of sight of the image sensor is lost 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 method may also include determining that a required yaw at a point on the coarse path is outside the minimum envelope.

[0009] In some examples, the method further includes sending, by the data processing hardware, a navigation route having a high-level waypoint and at least one intermediate waypoint to the low-level path generator. The method may also include 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.

[0010] Another aspect of the present disclosure provides a robot comprising a body and legs coupled to the body and configured to manipulate the robot in an environment. The robot also comprises 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 comprise receiving a navigation route. The navigation route comprises a series of high-level waypoints 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 location of static obstacles in an area to be navigated by the robot. The operations also comprise receiving image data of an environment surrounding the robot from an image sensor. The operations also comprise generating at least one intermediate waypoint based on the image data. The operations also comprise 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 along the series of high-level waypoints and the at least one intermediate waypoint toward the destination position.

[0011] This aspect 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 operation also includes maintaining each high-level path point in the series of high-level path points 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 robot's body cannot traverse. Additionally, generating at least one intermediate path point may include generating a sparse graph based on the body obstacle map. The sparse graph includes a series of nodes and edges. The nodes and edges represent a path 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 the image sensor along the rough path is lost, and generating one of the at least one intermediate path points at the point where the line of sight is lost.

[0012] In some embodiments, generating the sparse graph includes generating a fully configured spatial map based on the body obstacle map. The fully configured spatial map includes a two-dimensional grid of elements, wherein each element of the grid represents the space of the environment and each element of the fully configured spatial map includes a corresponding set of yaw configurations. Each yaw configuration can be classified as valid or invalid, wherein a valid yaw configuration represents a yaw configuration of a robot that is protected from the danger of contacting obstacles at the space associated with the corresponding element, and an invalid yaw configuration represents a yaw configuration of a robot that is not protected from the danger of contacting obstacles at the space associated with the corresponding element. The operation may also include generating a compressed configuration spatial map from the fully configured spatial map. The compressed configuration spatial map includes a second two-dimensional grid of elements, and each element of the second grid represents the space of the environment. Each element of the second grid is also classified as one of: (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 graph from a compressed configuration spatial map.

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

[0014] In some examples, generating a sparse graph further includes overlaying a plurality of Voronoi cells on the compressed configuration space map, wherein 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 a sparse graph further includes classifying each Voronoi cell as either an edge or a node. Classifying each Voronoi cell may include executing a flood fill algorithm.

[0015] In some embodiments, planning a coarse path from the first node to the second node includes trimmed edges, and each trimmed edge includes an element that is below a threshold length and is classified as either a yaw collision zone or a yaw constraint zone. Determining a point along the coarse path at which a line of sight of the image sensor is lost 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 also include determining that a required yaw at a point on the coarse path is outside the minimum envelope.

[0016] In some examples, the operation further includes sending a navigation route having a high-level waypoint and at least one intermediate waypoint to a low-level path generator. The operation may also include 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 the low-level path generator.

[0017] The details of one or more embodiments of the disclosure are set forth in the accompanying drawings and the description below. Other aspects, features, and advantages will be apparent from the description and drawings, and from the claims. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 is a schematic diagram of an example system for a navigation system that generates intermediate waypoints for a robot.

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

[0020] Figure 3 The robot uses high-level and intermediate waypoints to navigate Figure 2A and Figure 2B A schematic diagram of the navigation route.

[0021] Figure 4 yes Figure 1 Schematic diagram of an example intermediate waypoint generator for a navigation system.

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

[0023] Figure 6 yes Figure 5 Schematic diagram of the configuration space map overlaid with the Voronoi cells.

[0024] Figure 7 yes Figure 6 Schematic diagram of a Voronoi cell showing the distances between elements and the nearest obstacles to the Voronoi cell.

[0025] Figure 8A-8E Is filling Figure 6 Schematic diagram of the flood-fill algorithm for Voronoi cells.

[0026] Fig. 9 is from Figure 6 Schematic diagram of the edges and nodes derived from the Voronoi cells.

[0027] Fig.10 is from Figure 6 Schematic diagram of the pruned edges and nodes derived from the Voronoi cells.

[0028] Fig.11A and Fig. 11B is a two-dimensional sparse graph ( Fig.11A ) and SE2 sparse graph ( Fig. 11B ) schematic diagram.

[0029] Fig. 12A and Fig. 12B is a schematic diagram of the nodes added for the target destination.

[0030] Fig.13 is a schematic diagram of the path through the yaw free region and the yaw constrained region.

[0031] Fig.14 is a schematic diagram of the path through the yaw constraint region.

[0032] Fig.15A and Fig. 15B is a schematic diagram of the robot's navigation route that includes intermediate waypoints placed at locations where line of sight is lost.

[0033] Fig.16 is a flow chart of an example method for generating intermediate waypoints for a navigation system of a robot.

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

[0035] The same reference symbols in different drawings denote the same elements. DETAILED DESCRIPTION

[0036] 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. Typically, 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 target, the robot will typically first draw a path or road from this high-level map to navigate static obstacles, and then rely on a perception system that collects low-level navigation data to navigate around small and dynamic objects encountered while traveling. Typically, when navigating such environments, the robot will encounter obstacles that are not described 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. The embodiments of this article are directed to systems and methods for an intermediate waypoint generator that is used to generate intermediate waypoints to a high-level route in real time, thereby allowing a legged robotic device to effectively navigate around larger dynamic objects.

[0037] refer to Figure 1 , a 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 vertical gravity axis Vg along 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 posture 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 specific attitude or stance made by the robot 10. The posture of the robot 10 may be defined by the orientation or angular position of the robot 10 in space. Movement of the legs 12 relative to the body 11 changes the posture P of the robot 10 (ie, a combination of the position of the robot's CM and the posture or orientation of the robot 10).

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

[0039] The robot 10 also 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 surrounding the robot 10 at a viewing angle 32 and within a field of view 34. The vision system 30 can be configured to move the field of view 34 in any direction by adjusting the viewing angle 32 or by panning and / or tilting the camera 31 (either independently or via the robot 10). Alternatively, the vision system 30 can include multiple sensors or cameras 31 so that the vision system 30 captures a substantially 360 degree field of view of the robot 10 around.

[0040] In some embodiments, the 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) volume 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 the intermediate waypoint generator 120 and the low-level path generator 130.

[0041] The vision system 30 provides image data or sensor data 17 to the data processing hardware 36 of the robot 10, which is derived from the image data captured by the camera or sensor 31. The data processing hardware 36 is in digital communication 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.

[0042] In the illustrated example, the navigation system 100 includes a high-level waypoint generator 110 that receives map data 50 (i.e., high-level navigation data representing the locations of static obstacles in an area to be navigated by the robot 10) and generates one or more high-level waypoints 210 ( Figure 2A and 2B ), the navigation route 112 plots a rough path from the current location of the robot 10 to the target destination that bypasses large and / or static obstacles. For example, the high-level waypoint generator 110 can generate a rough path at 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, which 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 generate one or more intermediate waypoints 310 ( Figure 3 ) added to the advanced waypoint 210 of the navigation route 112 ( Figure 2A , Figure 2B and Figure 3 ), which forms a rough path or trajectory for the robot 10 to travel. The high-level waypoints 210 and any added intermediate waypoints 310 of the navigation route 112 are passed to the low-level path generator 130, which, in conjunction with the sensor data 17, generates a step plan 142 that plots each individual step of the robot 10 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 feet 19 or ends of the legs 12 on the ground 9 at the locations 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 toward the goal and the obstacle avoidance velocity.

[0043] 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 may 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 may receive the high-level waypoints 210 from the remote device. Alternatively, the entire navigation system 100 may be executed on the remote device, and the remote device may control / instruct the robot 10 to manipulate the environment 8 based on the step plan 142.

[0044] Reference now Figure 2A and Figure 2B , the low-level path generator 130 (using, for example, a potential field approach) may encounter problems when navigating obstacles of certain shapes and / or protrusions. For example, the low-level path generator 130 may slide off an obstacle or surface 220 and get stuck in a local minimum 230 when attempting to navigate to a high-level path point 210 of a corresponding navigation route 112 ( Figure 2A In another example, the low-level path generator 130 may lack the necessary context or decision-making capabilities regarding turning the robot 10. That is, the low-level path generator 130 may have difficulty distinguishing when to "turn and then drive" from "drive and then turn" ( Figure 2B ).

[0045] Reference now 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 appear convex to the low-level path generator 130 and allow the low-level path generator to successfully navigate around obstacles 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 jiggling. In addition, the intermediate waypoint generator 120 can avoid removing or deleting any high-level waypoints 210 because the high-level waypoint generator 110 can be aware of additional map context that is not available to the intermediate waypoint generator 120, and removing the high-level waypoints 210 may produce unpredictable results.

[0046] Reference now Figure 4In 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 cannot be traversed by the body 11 of the robot 10) in the immediate vicinity (e.g., within four meters) of the robot 10. 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 nodes 812 ( Figure 8A-8E ) between a series of valid interconnections and edges 810 ( Figure 8A-8E ). The nodes 812 and edges 810 represent valid potential paths through a yaw-constrained region (ie, an area of ​​difficult or tight maneuvering) near the robot 10, and thus the sparse graph 416S provides the information needed to determine potential paths for the robot 10 through the yaw-constrained region.

[0047] 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 from the starting location to the target destination ( Fig.13 ). 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 of the valid potential paths (i.e., the interconnections and edges 810 between a series of nodes 812) indicated by the sparse graph 416S. The coarse path planner 420 can pass the path 1310 to the waypoint placer 430. The waypoint placer 430 determines the location where the line of sight is lost when traveling along the path 1310 based on the current position of the robot 10, and places the intermediate waypoint at that location. That is, if the robot 10 looks along the coarse path 1310 from the current position, the waypoint placer determines the location where the robot 10 will lose the line of sight of the path (e.g., because of an obstacle), and places the intermediate waypoint 310 at the location on the path 1310 where the line of sight is lost. The intermediate waypoint 310 will guide the robot 10 to a location where the low-level path generator 130 can successfully navigate the obstacle. The intermediate waypoint 310 is included in the navigation route 112 and is passed to the low-level path generator 130.

[0048] In some embodiments, the sparse map generator 410 initially generates a full configuration space map 414 based on the received physical obstacle map 412. In some examples, the full configuration space map 414 includes a two-dimensional (2D) grid or array of elements 510, 510a-n, where each element 510 represents an area 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 full configuration space map 414 is an array of 128 elements by 128 elements. In some examples, each element 510 can be associated with a plurality of corresponding yaw configurations 512, 512a-n (corresponding yaw configuration sets 512, 512a-n), and the full configuration space map 414 can indicate the state of each corresponding yaw configuration 512 of each element 510. For example, each element 510 can be associated with 64 corresponding yaw configurations. In this example, each yaw configuration will be rotated 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 respective yaw configuration 512 associated with a respective element 510, the full configuration spatial map 414 may indicate whether the respective yaw configuration 512 would result in a collision with a body obstacle if the selected point of the robot 10 occupied the space represented by the element 510. That is, the full configuration spatial map 414 may indicate whether each yaw configuration 512 is valid (i.e., the robot 10 would not collide with an obstacle in the yaw configuration 512) or invalid (i.e., the robot 10 would collide with an obstacle in the yaw configuration 512). The selected point may, for example, include a center point of the body 11 of the robot 10 (e.g., equidistant from the top of each leg 12).

[0049] In some embodiments, the sparse map generator 410 generates a compressed configuration space map 500 from the full configuration space map 414. The compressed configuration space map 500 may classify each element 510 of the configuration space map 500, rather than associating each element with a plurality of yaw configurations 512. For example, each element 510 may be classified as a body obstacle element 510A ( Figure 5 )、Yaw collision element 510B( Figure 5 )、Yaw Free Element 510C( Figure 5 ) or yaw constraint element 510D ( Figure 5 That is, the yaw configuration 512 from the full configuration space map 414 may be used to classify each element 510 of the compressed configuration space map 500 .

[0050] Reference now Figure 5, an exemplary compressed configuration space map 500 is shown. In this example, the compressed configuration space map 500 contains a grid of 128 by 128 elements 510, each of which is classified as a body obstacle element 510, a yaw collision element 510B, a yaw freedom 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 traverse (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 all yaw configurations 512 (e.g., all 64 yaw configurations) of the robot 10 result in a collision with a body obstacle (i.e., the body obstacle element 510A) when a selected point of the robot 10 (e.g., the center of the body 11) occupies the space represented by the element 510B. For example, a space immediately adjacent to a wall, despite not actually containing a wall, may be too close to the wall for the robot 10 to stand without colliding with the wall.

[0051] 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 physical 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 with an axis at a selected point) without colliding with a physical 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 in those yaw configurations 512). For example, the robot 10 may be able to travel close to and 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., yaw configurations 512 in which the robot 10 is parallel to the wall) are safe, while others (i.e., yaw configurations 512 in which the robot 10 is perpendicular to the wall) are unsafe.

[0052] In some examples, the sparse map generator 410 determines a valid path through the yaw constraint zone 510DZ (ie, the region of the yaw constraint element 510D) of the compressed configuration space map 500. Referring now to Figure 6In some embodiments, the sparse map generator 410 overlays the compressed configuration space map 500 with a plurality of Voronoi cells 610. The Voronoi cell 610 includes all elements 510 in the yaw constraint zone 510DZ and / or the yaw free zone 510CZ that are equidistant from at least two elements 510 classified as yaw collision elements 510B. In other words, the Voronoi cell 610 is the element 510 that is farthest from the yaw collision zone 510BZ (and therefore farthest from the body obstacle). Alternatively, the Voronoi cell 610 includes all elements 510 in the yaw constraint zone 510DZ and / or the yaw free zone 510CZ that are equidistant from at least two body obstacle elements 510A. Figure 6 In the example of , the cell 610 is located at the center of the yaw free zone 510CZ and the yaw constrained zone 510DZ between the yaw collision zone 510BZ. The Voronoi cell 610 is easy to calculate based on data that the robot 10 can already access (e.g., a signed distance field).

[0053] To determine the elements 510 labeled as Voronoi cells 610, the sparse graph generator 410 determines the elements 510 (of the configuration space map 500) that are equidistant from obstacles (i.e., body obstacle elements 510A) on the configuration space map 500. That is, each element 510 inherently has a distance and direction to the nearest obstacle boundary. Figure 7 A portion of a configuration space map 500 is depicted, including two yaw collision zones 510BZ separated by a yaw constraint zone 510DZ. Some Voronoi cells 610 pass through the yaw constraint zone 510DZ and between the yaw collision zones 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 element 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 of the corresponding element. Whenever the distance between any two of the three nearest obstacle boundaries satisfies a threshold distance, element 510 may be labeled as a Voronoi cell 610. The threshold distance may be represented as any number of elements (eg, one element, two elements, five elements, etc.).

[0054] In the example shown, when the sparse map generator 410 analyzes the element 510 labeled "A", the sparse map generator 410 determines that the nearest yaw collision element 510B (or body obstacle element 510A) to element "A" is element 510a. The sparse map 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 element 510b. The sparse map generator 410 can then determine the distance from the bottom neighbor of the element (labeled "C") to the nearest yaw collision element 510B (in this case, also element 510a). In some examples, the sparse map generator 410 can determine whether two of the three nearest obstacle boundaries (i.e., two of the three distances between "A" and "B", between "A" and "C", and between "B" and "C") are within a threshold distance. The sparse map generator 410 may determine the distance between two elements when the elements have two obstacle boundaries that are the same distance apart.

[0055] 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, element "A" will be marked as a Voronoi cell 610.

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

[0057] Reference now Figure 8A-8E In some implementations, the sparse graph generator 410 determines edges 810 and nodes 812 derived from the Voronoi cells 610 . Fig. 8AAn exemplary Voronoi cell 610 in a yaw constraint zone 510DZ is shown. In some embodiments, the sparse graph generator selects an arbitrary (i.e., random or nearly random) element 510ff that is labeled as a Voronoi cell 610. Starting from an arbitrary element 510ff, the sparse graph generator performs a flood fill and records the distance from each element 510 labeled as a 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 that determines the area connected to a given node in an array. In this case, the flood fill continues to flow through all elements 510 labeled as Voronoi cells 610 until all elements are assigned distances. Although the arbitrarily selected element 510ff can be selected completely randomly, it can be advantageous to limit the selection of elements 510 near the robot 10 to minimize the impact of any errors.

[0058] After each element labeled as a Voronoi cell 610 is assigned a distance, in some examples, the sparse graph generator 410 determines the end or terminal node 812 of each branch of the Voronoi cell 610. The terminal node 812 of each branch will have a local maximum distance with an arbitrarily selected element 510ff. In some examples, the sparse graph generator 410 labels each element 510 starting from the terminal node 812 of the branch while moving back toward the selected element 510ff. Whenever the sparse graph generator 410 encounters an element 510 that has been previously labeled, 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 terminal node 812a and moves back toward the selected element 510ff, marking each element along the way to form an edge 810a. That is, the Voronoi cells 610 from the terminal node 812a to the selected element 510ff may form an edge 810a. The sparse graph generator 410 may continue this process until all elements have been marked. For example, in Figure 8C , sparse graph generator 410 proceeds from end node 812b toward element 510ff and marks each element to form another edge 810b until a previously marked element ( Figure 8B ). The sparse graph generator 410 may assign a unique label to the junction element (i.e., the first element encountered that has been previously labeled) at 812c. The sparse graph generator 410 may continue to proceed back until it reaches the selected element 510ff, noting each element of the edge 810c that was previously labeled as separated.

[0059] Reference now Fig.8D, the sparse graph generator 410 may start at the end node 812d and go back to the element 510ff, again marking each element along the way to form another edge 810d. After all elements 510 have been marked (and assigned to edges 810), in some embodiments, the sparse graph generator 410 determines whether the selected element 510ff only touches two edges 810 (i.e., is not a connection point between three or more edges). In this case, the sparse graph generator 410 may combine the two edges into a single edge. For example, if Fig.8D As shown, the selected element 510ff touches only two other edges (i.e., edge 810c and edge 810d), and Fig. 8E , edge 810c and edge 810d are combined into a single edge 810. The final result of flood filling and element labeling provides the sparse graph generator 410 with an ordered set of elements 510 along edge 810 and a series of node elements 812. The series of nodes and edges form a sparse graph 416S and represent valid paths that the robot 10 can travel through the constraint space of the nearby environment (i.e., the yaw constraint zone 510DZ of the compressed configuration space map 500).

[0060] Fig. 9 The configuration space map 500 is shown overlaid with Voronoi cells 610 classified into a plurality of edges 810 and nodes 812, each edge 810 connected to either another edge 810 (through the node 812) or to the yaw free zone 510CZ (also through the 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 located in the yaw constrained zone 510DZ, because the path through the yaw free zone 510DZ can be determined later. As shown in reference Figure 8A-8EAs discussed, the sparse graph generator 410 may split the 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 at which edges 810 connect to the yaw free zone 510CZ. That is, the sparse graph generator 410 identifies nodes 812 (i.e., ends or edges 810 or connection points between edges 810) along the Voronoi cells 610 connecting the yaw free zone 510CZ and the yaw constrained zone 510DZ. The sparse graph generator 410 may also identify nodes 812 that are connection points for 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 nodes 812 (i.e., connected to the same yaw free zone 510CZ), the sparse graph generator may (at least temporarily) ignore motion through the yaw free zone 510CZ. With the edges 810 and nodes 812, the sparse graph generator 410 can first generate a final 2D configuration map 1000 ( FIG. 1000 ) based on the previously determined edges 810 and nodes 812 in the compressed configuration space map 500 that overlap the Voronoi cells 610 by pruning away the edges 810 that do not have navigational significance. Fig.10 ).For example, Fig.10 A compressed configuration space map 500 is shown, which is covered with a plurality of Voronoi cells 610 and has a plurality of insignificant edges 1010 that are pruned to generate a 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 a yaw constraint zone 510DZ, because these paths are highly unlikely to be worth traversing by the robot 10. Alternatively, the threshold length may be approximately the radius of the robot 10. In some examples, when, after pruning, a connection point node 812 is connected to fewer than three edges 810, the sparse graph generator 410 groups the edges 810 together.

[0061] Reference now Fig.11A and Fig. 11B In some embodiments, the sparse map generator 410 converts the final 2D configuration map 1000 ( Fig.11A ) is converted into the SE2 representation of the figure 1100 ( Fig. 11B ). SE2 is a coordinate system that includes yaw with standard Cartesian coordinates (ie, x-coordinate, y-coordinate, and yaw). That is, SE2 map 1100 adopts the final 2D configuration map 1000 and includes yaw. Fig. 11BAn SE2 graph 1100 is shown where yaw is altitude. Although the robot 10 can have any number of yaw configurations 512 (e.g., 64 as previously discussed), the SE2 graph 1100 can be simplified by considering only two yaw configurations - forward and backward. That is, typically, the robot 10 will travel in a yaw configuration 512 that is aligned with its path of travel, and therefore, the SE2 graph 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 ( Fig.11A ) is connected to twice the number of edges 1110 of the number of nodes 812 in the 2D map 1000 representation ( Fig. 11B ). For example, Fig.11A Three edges 810 are shown connected at a single node 812, which translates to six nodes 1112 in the SE2 graph 1100. This means that the robot 10 will rotate in place at the node 1112, and the sparse graph generator 410 can determine the interconnections between the nodes 1112 based on the valid yaw configuration 512 from the full configuration space map 414.

[0062] To finalize the SE2 map 1100, in some embodiments, the sparse map generator 410 determines entry and exit points for the robot 10 from the map 1100. Fig. 12A and Fig. 12B , the sparse graph generator 410 may project an off-map target 1210 (i.e., a target destination) to the boundary of the map 1100 along a straight line. When the target 1210 is set in the yaw free zone 510CZ, the sparse graph generator 410 may use a corresponding element associated with the target 1210. However, when in the yaw constraint zone 510DZ, the sparse graph generator 410 may search along the nearest element 510 that is 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 may set the element as the target. When the nearest element is marked as a Voronoi cell 610 on an edge 810, the sparse graph generator 410 may split the edge 810 in two by placing a new node 1220 at the corresponding nearest element. Fig. 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 splits the previous edge 810 into two separate edges 810. In some embodiments, the sparse graph generator 410 creates two new nodes 1220, one node 1220 representing the forward yaw configuration 512 and one node 1220 representing the backward yaw configuration 512.

[0063] 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. Fig. 12B In 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 robot's location. 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 robot's location. 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 robot's starting location. That is, the robot 10 can travel either forward or backward to the location of the node 1220.

[0064] In some examples, the sparse graph generator 410 connects nodes 812 that indicate the robot 10 will collide with an obstacle. For example, the sparse graph generator 410 may always assume that the robot turns by rotating from a center point, which may make some turns impossible. However, in some cases, the low-level path generator 130 may still be able to navigate a path by opening a pivot point that is not at the center of the robot 10, and in these cases, the sparse graph generator 410 may create edges 810 between nodes 812 that the generator 120 may otherwise determine that the robot cannot traverse.

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

[0066] To generate the path 1310, the coarse path planner 420 may implement a search algorithm (e.g., an A* search algorithm). The coarse path planner 420 may use the search algorithm to search a dense grid or graph 416D of the 2D yaw free zone 510CZ to draw the path 1310. The sparse graph generator 410 may also generate a dense graph 416D and combine it with the sparse graph 416S and send the combined graph to the coarse path planner 420. Alternatively, the coarse path planner may generate the dense graph 416D. The dense graph is a list of interconnected nodes of the yaw free zone 510CZ determined from the compressed configuration 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. Because 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 (which is dense relative to 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 interconnected nodes within both the yaw free zone 510CZ and the yaw constrained zone 510DZ. The coarse path planner 420 can also use a search algorithm to search the final graph 416F to find Voronoi cells 610 in the yaw constrained zone 510DZ and the yaw free zone 510CZ with contained orientation information. The coarse path planner 420 can link the yaw free zone 510CZ with the yaw constrained zone 510DZ and traverse the yaw constrained zone 510DZ with the coarse path 1310.

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

[0068] For example, now refer to Fig.14 1100, the coarse path planner 420 has planned a path 1430 from a starting position 1410 (having an orientation aligned with a corresponding arrow) to a target position 1420 (having a direction aligned with a corresponding arrow). Due to the forward bias, the search algorithm may select a forward-facing orientation (path AC) that passes through the yaw constraint zone 510DZ. The coarse path planner 420 optionally calculates the cost of path AC (facing forward) and path BD (facing backward). In the given example, because the given starting and target orientations are facing backward, the backward path of BD is cheaper (i.e., requires fewer rotations) than the forward-facing path AC, and the coarse path planner 420 may update the path 1430 accordingly.

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

[0070] Reference now Fig.15A, the robot 10 navigates along the path 1310 through the yaw constraint zone 510DZ located between the two body obstacle zones 510AZ. The waypoint placer 430 draws 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 portion of the body obstacle zone 510 (or, alternatively, is within a threshold distance of the body obstacle zone 510 to account for the width of the robot 10), the waypoint placer 430 places an intermediate waypoint 310 on the path 1310 at a point 1530 where the ray 1510 intersects the path 1310 or just before the line of sight is lost. Therefore, it is relatively simple to determine the location 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.

[0071] refer to Fig. 15B , to account for the included yaw, the waypoint placer 430 can determine the minimum allowed yaw and the maximum allowed yaw at each point along the downsampled path 1310 to generate the envelopes 1550, 1550a-n. In some examples, the waypoint placer 430 tracks the smallest (i.e., innermost) envelope 1560, 1560a-n at each downsampled point and places the intermediate waypoint 310 when either the envelope width drops below the envelope threshold or the desired yaw configuration at the point 1530 on the path 1310 is outside the envelope 1550. In the example shown, the yaw of the envelope 1550c is outside the inner envelope 1560c and is a candidate for the intermediate waypoint 310.

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

[0073] The high-level waypoint 210 may include two coordinates indicating a location on a plane (i.e., an x ​​and y coordinate), a yaw value, and a time value. The time value may 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 may similarly add an estimated time value to the intermediate waypoint 310. The estimated time value may be determined by considering the maximum amount of time from the starting position to the waypoint in x, y and yaw using any speed constraints received. The waypoint placer 430 may offset the time for each high-level waypoint 210 on the planned trajectory after the intermediate waypoint 310.

[0074] Return to 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 step plan 142 for navigating to each waypoint 210, 310. In the event that the intermediate waypoint generator 120 determines that the intermediate waypoint 310 is not necessary (or fails to find an appropriate location), the intermediate waypoint generator 120 may pass the received navigation route 112 to the low-level path generator 130 intact.

[0075] Fig.16 is a flow chart 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 location 113 and end at a destination location 114. The navigation route 112 is based on high-level navigation data 50 that represents the locations of static obstacles in an area that the robot 10 is to navigate.

[0076] The method 1600 includes, at operation 1604, receiving, at the data processing hardware 36, image data 17 of an environment 8 surrounding the robot 10 from the image sensor 31. 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 location 113 toward the destination location 114 along the series of high-level waypoints 210 and the at least one intermediate waypoint 310.

[0077] Fig.171 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 meant to be exemplary only and are not meant to limit implementations of the inventions described and / or claimed in this document.

[0078] 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 are 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 may be used, as well as multiple memories and types of memories, as appropriate. In addition, multiple computing devices 1700 may be connected, with each device providing portions of the necessary operations (e.g., as a server bank, a group of blade servers, or a multi-processor system). Processor 1710 may execute Figure 1 and 4 An intermediate waypoint generator 120 is provided.

[0079] The memory 1720 stores information non-temporarily within the computing device 1700. The memory 1720 may be a computer-readable medium, a volatile storage unit, or a non-volatile storage unit. The non-temporary memory 1720 may be a physical device for temporarily or permanently storing programs (e.g., instruction sequences) 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 bootloaders). 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 disk or tape.

[0080] Storage device 1730 can provide a large amount of storage for computing device 1700. In some embodiments, storage device 1730 is a computer-readable medium. In various different embodiments, storage device 1730 can be a floppy disk device, a hard disk device, an optical disk device or a tape device, a flash memory or other similar solid-state memory device, or a device array, including devices in a storage area network or other configuration. 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 memory 1720, storage device 1730, or memory on processor 1710.

[0081] The high-speed controller 1740 manages bandwidth-intensive operations of the computing device 1700, while the low-speed controller 1760 manages less bandwidth-intensive operations. Such a division of responsibilities is exemplary only. In some embodiments, the high-speed controller 1740 is coupled to the memory 1720 and the high-speed expansion port 1750, which 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.

[0082] Various implementations of the systems and techniques described herein can be implemented in digital electronic and / or optical circuits, integrated circuits, specially designed application specific integrated circuits (ASICs), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include implementations 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.

[0083] These computer programs (also referred to as programs, software, software applications, or code) contain machine instructions for a programmable processor and may 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., disk, optical disk, memory, programmable logic device (PLD)) for providing machine instructions and / or data to a programmable processor, including machine-readable media that receive machine instructions as machine-readable signals. The term "machine-readable signal" refers to any signal for providing machine instructions and / or data to a programmable processor.

[0084] The processes and logic flows described in this specification can be performed by one or more programmable processors (also referred to as data processing hardware), which execute one or more computer programs to perform functions by operating on input data and generating outputs. These processes and logic flows can also be performed by special logic circuits such as field programmable gate arrays (FPGAs) or application specific integrated circuits (ASICs). For example, processors suitable for executing computer programs include both general and special microprocessors, and any one or more processors of any type of digital computer. Typically, the processor will receive instructions and data from a read-only memory or a random access memory or both. The basic elements of a computer are a processor for executing instructions and one or more storage devices for storing instructions and data. Typically, a computer will also include or be operably coupled to receive data from or transmit data to one or more large-capacity storage devices (such as disks, magneto-optical disks, or optical disks) for storing data, or both. However, a computer does not need to have such a device. Computer readable media suitable for storing computer program instructions and data include all forms of non-volatile memory, media and storage devices, including, for 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 ROMs and DVD-ROMs. The processor and memory may be supplemented by or incorporated in special purpose logic circuits.

[0085] A number of embodiments have been described. However, it will be appreciated that various modifications may 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 method (1600) for generating intermediate waypoints, comprising: receiving, at data processing hardware (36) of a robot (10), a navigation route (112) comprising a series of waypoints (210), wherein the navigation route (112) is based on navigation data (50) representing one or more locations of one or more obstacles in an environment (8) of the robot (10); receiving, at the data processing hardware (36), image data (17) associated with an environment (8) from an image sensor (31); generating, by the data processing hardware (36), at least one intermediate waypoint (310) based on the image data (17); providing the series of waypoints (210) and the at least one intermediate waypoint (310) to a path generator (130) by the data processing hardware (36); receiving, by the data processing hardware (36), a step plan (142) from the path generator (130), the step plan (142) indicating one or more locations for placing one or more feet of the robot (10) based on the series of paths (210) and the at least one intermediate path (310); as well as The data processing hardware (36) uses the step plan (142) to instruct the robot (10) to navigate from a starting location (113) via the series of waypoints (210) and the at least one intermediate waypoint (310) to a destination location (114).

2. The method (1600) of claim 1, further comprising receiving an obstacle map (412), the obstacle map (412) identifying obstacles in the environment (8), and in, Generating the at least one intermediate waypoint (310) includes: generating a first graph (416S) based on the obstacle map (412), the first graph (416S) comprising a set of nodes (812) and one or more edges (810), the set of nodes (812) and the one or more edges (810) representing one or more paths that the robot (10) can travel in the environment (8); planning a path (1310) from a first node (812) of the set of nodes (812) to a second node (812) of the set of nodes (812), the first node (812) and the second node (812) each representing a space in the environment (8); determining a point (1530) at which a line of sight of an image sensor (31) associated with the path (1310) intersects the obstacle; and An intermediate waypoint of the at least one intermediate waypoint (310) is generated at the point (1530).

3. The method (1600) of claim 2, wherein: Generating the first graph (416S) includes: generating a map (414) based on the obstacle map (412), the map (414) comprising a first two-dimensional grid of elements (510), each element (510) of the first two-dimensional grid of elements (510) being associated with a corresponding set of yaw configurations (512) of the robot (10); generating a compressed map (500) from the map (414), the compressed map (500) comprising a second two-dimensional grid of elements (510); and The first map (416) is generated based on the compressed map (500).

4. The method (1600) of claim 3, wherein: Planning a path (1310) from the first node (812) to the second node (812) includes: generating a second map (416D) based on the compressed map (500), the second map (416D) comprising one or more elements (510) of the second two-dimensional grid that are classified as elements (510) of a yaw free zone (510CZ); and One or more edges (810) from the first graph (416S) are linked to the one or more elements (510).

5. The method (1600) of claim 4, wherein: Linking the one or more edges (810) to the one or more elements (510) includes: combining the first graph (416S) and the second graph (416D) to generate a third graph (416F); and A search algorithm is performed on the third graph (416F).

6. The method (1600) of claim 3, wherein: Generating the first map (416) also includes overlaying a plurality of Voronoi cells (610) onto the compressed map (500).

7. The method (1600) of claim 6, wherein: Generating the first graph (416S) also includes classifying each Voronoi cell (610) of the plurality of Voronoi cells (610) as either an edge (810) or a node (812).

8. The method (1600) of claim 7, wherein: Classifying each Voronoi cell (610) of the plurality of Voronoi cells (610) includes performing a flood fill algorithm.

9. The method (1600) of claim 3, wherein: Planning the path (1310) from the first node (812) to the second node (812) includes pruning one or more edges (810).

10. The method (1600) of claim 2, wherein: Determining the point (1530) includes: determining a minimum yaw and a maximum yaw (1550) associated with the path (1310); determining an envelope based on the minimum yaw and the maximum yaw (1560); and It is determined that the yaw at the point (1530) is outside the envelope (1560).

11. A method (1600) according to any one of claims 1-10, wherein each waypoint in the series of waypoints (210) and each intermediate waypoint in the at least one intermediate waypoint (310) is associated with one or more corresponding 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 (10) to navigate to the corresponding waypoint (210, 310).

12. The method (1600) of any one of claims 1-10, further comprising maintaining each waypoint in the series of waypoints (210) within the navigation route (112).

13. The method (1600) according to any one of claims 1-10, wherein the at least one intermediate waypoint (310) is added to the navigation route (112) between a first waypoint of the series of waypoints (210) and a second waypoint of the series of waypoints (210).

14. The method (1600) according to any one of claims 1-10, further comprising: determining whether to add an intermediate waypoint to the navigation route (112); Wherein providing the series of path points (210) and the at least one intermediate path point (310) to the path generator (130) comprises: In response to determining not to add the intermediate waypoint to the navigation route (112), the series of waypoints (210) and the at least one intermediate waypoint (310) without the intermediate waypoint are provided to a route generator (130).

15. A robot (10), comprising: Body (11); legs (12) coupled to the body (11) and configured to maneuver the robot (10) around an environment (8); data processing hardware (36), the processing hardware (36) communicating with the leg (12); and Memory hardware (38) in communication with the data processing hardware (36), the memory hardware (38) storing instructions that, when executed on the data processing hardware (36), cause the data processing hardware (36) to perform operations including: receiving a navigation route (112) comprising a series of waypoints (210), wherein the navigation route (112) is based on navigation data (50) representing one or more locations of one or more obstacles in an environment (8) of the robot (10); receiving image data (17) associated with an environment (8) from an image sensor (31); generating at least one intermediate waypoint (310) based on the image data (17); providing the series of waypoints (210) and the at least one intermediate waypoint (310) to a path generator (130); receiving a step plan (142) from the path generator (130), the step plan (142) indicating one or more locations for placing one or more feet of the robot (10) based on the series of paths (210) and the at least one intermediate path (310); and The step plan (142) is used to instruct navigation of the robot (10) from a starting location (113) via the series of waypoints (210) and the at least one intermediate waypoint (310) to a destination location (114).

16. The robot (10) of claim 15, wherein the operations further comprise receiving an obstacle map (412), the obstacle map (412) identifying obstacles in the environment (8), and in, Generating the at least one intermediate waypoint (310) includes: generating a first graph (416S) based on the physical obstacle map (412), the first graph (416S) comprising a set of nodes (812) and one or more edges (810), the set of nodes (812) and the one or more edges (810) representing one or more paths that the robot (10) can travel in the environment (8); planning a path (1310) from a first node (812) of the set of nodes (812) to a second node (812) of the set of nodes (812), the first node (812) and the second node (812) each representing a space in the environment (8); determining a point (1530) at which a line of sight of an image sensor (31) associated with the path (1310) intersects the obstacle; and An intermediate waypoint of the at least one intermediate waypoint (310) is generated at the point (1530).

17. The robot (10) according to claim 16, wherein: Generating the first graph (416S) includes: generating a map (414) based on the obstacle map (412), the map (414) comprising a first two-dimensional grid of elements (510), each element (510) of the first two-dimensional grid of elements (510) being associated with a corresponding set of yaw configurations (512) of the robot (10); generating a compressed map (500) from the map (414), the compressed map (500) comprising a second two-dimensional grid of elements (510); and The first map (416) is generated based on the compressed map (500).

18. The robot (10) according to claim 17, wherein: Planning a path (1310) from the first node (812) to the second node (812) includes: generating a second map (416D) based on the compressed map (500), the second map (416D) comprising one or more elements (510) of the second two-dimensional grid that are classified as elements (510) of a yaw free zone (510CZ); and One or more edges (810) from the first graph (416S) are linked to the one or more elements (510).

19. The robot (10) according to claim 18, wherein: Linking the one or more edges (810) to the one or more elements (510) includes: combining the first graph (416S) and the second graph (416D) to generate a third graph (416F); and A search algorithm is performed on the third graph (416F).

20. The robot (10) according to claim 17, wherein: Generating the first map (416) also includes overlaying a plurality of Voronoi cells (610) onto the compressed map (500).

21. The robot (10) of claim 20, wherein generating the first graph (416S) further comprises classifying each Voronoi cell (610) of the plurality of Voronoi cells (610) as either an edge (810) or a node (812).

22. The robot (10) according to claim 21, wherein: Classifying each Voronoi cell (610) of the plurality of Voronoi cells (610) includes performing a flood fill algorithm.

23. The robot (10) of claim 17, wherein: Planning the path (1310) from the first node (812) to the second node (812) includes pruning one or more edges (810).

24. The robot (10) of claim 16, wherein: Determining the point (1530) includes: determining a minimum yaw and a maximum yaw (1550) associated with the path (1310); determining an envelope based on the minimum yaw and the maximum yaw (1560); and It is determined that the yaw at the point (1530) is outside the envelope (1560).

25. The robot (10) according to any one of claims 15-24, wherein each waypoint in the series of waypoints (210) and each intermediate waypoint in the at least one intermediate waypoint (310) is associated with one or more corresponding coordinates indicating a position on a plane, a yaw value, and a time value, wherein the time value indicates an estimated amount of time for the robot (10) to navigate to the corresponding waypoint (210, 310).

26. The robot (10) of any one of claims 15-24, the operations further comprising maintaining each waypoint in the series of waypoints (210) within the navigation route (112).

27. The robot (10) according to any one of claims 15-24, wherein the at least one intermediate waypoint (310) is added to the navigation route (112) between a first waypoint of the series of waypoints (210) and a second waypoint of the series of waypoints (210).

28. The robot (10) according to any one of claims 15-24, wherein the operation further comprises: determining whether to add an intermediate waypoint (310) to the navigation route (112); Wherein providing the series of path points (210) and the at least one intermediate path point (310) to the path generator (130) comprises: In response to determining not to add the intermediate waypoint to the navigation route (112), the series of waypoints (210) and the at least one intermediate waypoint (310) without the intermediate waypoint are provided to a route generator (130).

Images