Patent Description:
Robotic devices are increasingly being used to navigate constrained environments to perform a variety of tasks or functions. These robotic devices often need to navigate through these constrained environments without contacting the obstacles or becoming 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 dynamic obstacles while successfully navigating to the destination.

<CIT> describes a system for controlling a motion of an object from an initial location to a final location within a region while avoiding a set of obstacles located in the region.

<CIT> describes a robot including a drive system configured to maneuver the robot about the environment.

<NPL>, describes a robot using model predictive contouring control for collision avoidance.

The present invention relates to a method and a system in accordance with the appended claims.

One aspect of the disclosure provides a method for constraining robot autonomy language. The method includes receiving, at data processing hardware, a navigation command to navigate a robot to a mission destination within an environment of the robot. The method also includes generating, by the data processing hardware, a route specification for navigating the robot from a current location in the environment to the mission destination in the environment. The route specification includes a series of route segments. Each route segment in the series of route segments includes a goal region for the corresponding route segment, a constraint region encompassing the goal region, and an initial path for the robot to follow while traversing the corresponding route segment. The constraint region establishes boundaries for the robot to remain within while traversing toward the goal region.

Implementations of the disclosure may include one or more of the following optional features. In some implementations, the constraint region of each route segment in the series of route segments overlaps at least one other constraint region of another route segment in the series of route segments. In some examples, the constraint region encompassing the goal region for the corresponding route segment also encompasses the goal region associated with a previous route segment in the series of route segments that overlaps the corresponding route segment.

Optionally, each goal region comprises a convex shape. Each goal region may represent an area that a center point of the robot enters while traversing the route segment. Each route segment may further include at least one of goal costs, velocity bounds, position costs, position constraints, velocity costs, yaw constraints, or mobility parameters.

In some implementations, the mobility parameters include a categorization of a surface within the corresponding route segment. The categorization may include stairs or flat ground. In some examples, the velocity bounds include a minimum velocity for the robot while traversing the corresponding route segment and a maximum velocity for the robot while traversing the corresponding route segment. The velocity bounds may include angular velocity bounds, lateral velocity bounds, and longitudinal velocity bounds.

In some examples, the robot sequentially traverses each route segment in the series of route segments by passing through each respective goal region while navigating to the mission destination in the environment. The route specification may include at least one global constraint configured to constrain the robot while traversing each route segment. The data processing hardware, in some implementations, resides on the robot. Receiving the navigation command, in some examples, includes receiving the navigation command from a user device in communication with the data processing hardware. Optionally, the goal region for the last route segment in the series of route segments encompasses the mission destination.

Another aspect of the disclosure provides a system for constraining robot autonomy language. The system includes data processing hardware 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 that include receiving a navigation command to navigate a robot to a mission destination within an environment of the robot. The operations also include generating a route specification for navigating the robot from a current location in the environment to the mission destination in the environment. The route specification includes a series of route segments. Each route segment in the series of route segments includes a goal region for the corresponding route segment, a constraint region encompassing the goal region, and an initial path for the robot to follow while traversing the corresponding route segment. The constraint region establishes boundaries for the robot to remain within while traversing toward the goal region.

This aspect may include one or more of the following optional features. In some implementations, the constraint region of each route segment in the series of route segments overlaps at least one other constraint region of another route segment in the series of route segments. In some examples, the constraint region encompassing the goal region for the corresponding route segment also encompasses the goal region associated with a previous route segment in the series of route segments that overlaps the corresponding route segment.

As robotic devices (also referred to as "robots") become more prevalent, there is an increasing need for the robots to autonomously navigate environments that are constrained in a number of ways. Often, the robots rely on high-level map data that stores information relating to large and/or static objects (e.g., walls, doors, etc.). When given a destination goal, the robot will often first plot an initial path or course from this high-level map using a high-level navigation system to navigate the static obstacles and then rely on a local navigation system that gathers local navigation data to navigate around small and dynamic objects encountered while travelling.

While navigating, the robot requires a way for the high-level components of the navigation system, which have access to large-scale navigation information, to communicate goals, context, and intentions to lower-level navigation and obstacle avoidance systems so that the lower-level systems (i.e., local systems) may make more informed decisions about how to locally navigate the robot in a safe and efficient manner. Conventional techniques of representing goals as single poses lose the required context needed by the lower levels to make these optimizations and decisions. By including this context, the lower-level navigation systems can navigate the robot in a safe manner while still achieving the goals set by the high-level navigation system.

Typically, the high-level navigation system, when providing the initial path to the local navigation system (i.e., the navigation system that operates on a small, local map of the robot's environment versus a building-scale map of the high-level navigation system), most, if not all, context related to the path and destination is lost. That is, conventional techniques may specify the goals of a route, but the execution of the route is limited in how the local navigation system can respond to disturbances or obstacles along its path. The local navigation system lacks the context or information to determine whether a particular deviation from the goal is acceptable or even preferred. As a result, conventional techniques output local trajectories directly based on the map and route details, as well as the state of the robot. This over-constrains the systems that follow this trajectory (e.g., the local navigation system), because the systems are not aware of the amount of freedom available to deviate from the trajectory.

Implementations herein are directed toward systems and methods for a navigation system that utilizes a route specification language that includes additional context in a compact representation that allows a local navigation system (e.g., obstacle avoidance systems) to make appropriate decisions and optimizations. For example, the route specification language may represent constraints of staircases, including yaw and position. The route specification language may represent how near the robot must get to previously recorded sensor data in order to localize to the sensor data. In yet another example, the language may specify that the robot should not change its yaw or orientations in certain situations or locations.

By including relevant context and navigational information, the robot may perform tasks in a safer manner. For example, the robot provides larger safety margins and smoother paths, because the local navigation system has the necessary information to know acceptable ways to steer while still achieving the goals commanded by the higher-level navigation system. Additionally, the language enables new applications to be built by allowing new systems to specify goals and requirements in a more precise manner than was previously possible, while still allowing the local navigation system the freedom to adjust the robot in ways that do not affect the goals and requirements.

Referring to <FIG>, a robot or robotic device <NUM> includes a body <NUM> with two or more legs <NUM> and executes a navigation system <NUM> for enabling the robot <NUM> to navigate a constrained environment <NUM>. Each leg <NUM> is coupled to the body <NUM> and may have an upper portion <NUM> and a lower portion <NUM> separated by a leg joint <NUM>. The lower portion <NUM> of each leg <NUM> ends in a foot <NUM>. The foot <NUM> of each leg is optional and the terminal end of the lower portion of one or more of the leg <NUM> may be coupled to a wheel or the distal end of each leg <NUM> may directly contact the a ground surface <NUM>. The robot <NUM> has a vertical gravitational axis Vg along a direction of gravity, and a center of mass CM, which is a point where the weighted relative position of the distributed mass of the robot <NUM> sums to zero. The robot <NUM> further has a pose P based on the CM relative to the vertical gravitational axis Vg (i.e., the fixed reference frame with respect to gravity) to define a particular attitude or stance assumed by the robot <NUM>. The attitude of the robot <NUM> can be defined by an orientation or an angular position of the robot <NUM> in space. Movement by the legs <NUM> relative to the body <NUM> alters the pose P of the robot <NUM> (i.e., the combination of the position of the CM of the robot and the attitude or orientation of the robot <NUM>).

In some implementations, the robot <NUM> further includes one or more appendages, such as an articulated arm <NUM> disposed on the body <NUM> and configured to move relative to the body <NUM>. The articulated arm <NUM> may have five-degrees or more of freedom. Moreover, the articulated arm <NUM> may be interchangeably referred to as a manipulator arm or simply an appendage. In the example shown, the articulated arm <NUM> includes two portions <NUM>, <NUM> rotatable relative to one another and also the body <NUM>; however, the articulated arm <NUM> may include more or less portions without departing from the scope of the present disclosure. The first portion <NUM> may be separated from second portion <NUM> by an articulated arm joint <NUM>. An end effector <NUM>, which may be interchangeably referred to as a manipulator head <NUM>, may be coupled to a distal end of the second portion <NUM> of the articulated arm <NUM> and may include one or more actuators <NUM> for gripping/grasping objects.

The robot <NUM> also includes a vision system <NUM> with at least one imaging sensor or camera <NUM>, each sensor or camera <NUM> capturing image data or sensor data <NUM> of the environment <NUM> surrounding the robot <NUM> with an angle of view <NUM> and within a field of view <NUM>. The vision system <NUM> may be configured to move the field of view <NUM> by adjusting the angle of view <NUM> or by panning and/or tilting (either independently or via the robot <NUM>) the camera <NUM> to move the field of view <NUM> in any direction. Alternatively, the vision system <NUM> may include multiple sensors or cameras <NUM> such that the vision system <NUM> captures a generally <NUM>-degree field of view around the robot <NUM>.

The camera(s) <NUM> of the vision system <NUM>, in some implementations, include one or more stereo cameras (e.g., one or more RGBD stereo cameras). In other examples, the vision system <NUM> 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 such combination of sensors).

The vision system <NUM> provides image data or sensor data <NUM> captured by the cameras or sensors <NUM> to data processing hardware <NUM> of the robot <NUM>. The data processing hardware <NUM> is in digital communication with memory hardware <NUM> that stores instructions executable by the data processing hardware for performing operations. While the example shown depicts the data processing hardware <NUM> and the memory hardware <NUM> residing on the robot <NUM>, other configurations may include the data processing hardware <NUM> and the memory hardware <NUM> implemented on a remote system in communication with the robot <NUM>. The remote system may be a single computer, multiple computers, or a distributed system (e.g., a cloud environment) having scalable / elastic computing resources and/or storage resources. A navigation system <NUM> of the robot <NUM> executes on the data processing hardware <NUM>. The navigation system <NUM> may include a high-level navigation system <NUM> and a local navigation system <NUM>.

In some implementations, the navigation system <NUM> receives a navigation command <NUM> to navigate the robot <NUM> to a mission destination <NUM> (also referred herein as a target destination <NUM>) within the environment <NUM> of the robot <NUM>. The navigation command <NUM> may specify mission parameters for performing one or more actions/behaviors by the robot <NUM> at the mission destination <NUM>. The navigation system <NUM> may receive the navigation command <NUM> from a user device <NUM> associated with a user <NUM>. The user <NUM> may use the user device <NUM> to control/navigate the robot <NUM> around the environment <NUM> and may be any appropriate device such as a tablet, a mobile phone, a laptop or other computing system, or a dedicated controller. The mission destination <NUM> indicates a target location within the environment <NUM> that the navigation system <NUM> must navigate the robot <NUM> to. In the example shown, the mission destination <NUM> indicates a location in front of a door <NUM> in preparation for the robot <NUM> to open the door <NUM>.

In some implementations, the navigation system <NUM> includes the high-level navigation system <NUM> that receives map data <NUM> (i.e., high-level navigation data representative of locations of static obstacles in an area the robot <NUM> is to navigate). The high-level navigation system <NUM> may use the map data <NUM> and the navigation command <NUM> to generate a route specification <NUM> for navigating the robot <NUM> from a current location <NUM> to the mission destination <NUM>. The route specification <NUM> generated by the high-level navigation system <NUM> includes a series of route segments <NUM>, 210a-n (<FIG>), with each route segment <NUM> including an initial path <NUM> (<FIG>) for the robot <NUM> to follow while traversing the corresponding route segment <NUM>. Each segment <NUM>, as discussed in more detail below, may include additional constraints <NUM> for constraining the robot <NUM> while traversing the environment <NUM>.

With continued reference to <FIG>, the high-level navigation system <NUM> sends the route specification <NUM> to the local navigation system <NUM>. The local navigation system <NUM>, based on the route specification <NUM> and the sensor data <NUM>, generates a step plan <NUM> that plots each individual step of the robot <NUM> to navigate from the current location <NUM> of the robot <NUM> to the mission destination <NUM>. Using the step plan <NUM>, the robot <NUM> maneuvers through the environment <NUM> (e.g., around local and dynamic obstacles) by placing the feet <NUM> or distal ends of the leg <NUM> on the ground surface <NUM> at locations specified by the step plan <NUM>.

In some examples, at least a portion of the navigation system <NUM> executes on a remote device in communication with the robot <NUM>. For instance, the high-level navigation system <NUM> may execute on a remote device to generate the route specification <NUM> and the local navigation system <NUM> may execute on the robot <NUM> to receive the route specification <NUM> from the remote device. Optionally, the entire navigation system <NUM> may execute on a remote device and the remote device may control/instruct the robot <NUM> to maneuver the environment <NUM> based the step plan <NUM>. In additional examples, the remote device includes a remote computing device (e.g., a cloud-based server) in communication with the user device <NUM> and the robot <NUM>.

Referring now to <FIG>, an exemplary route specification <NUM> provides the local navigation system <NUM> with context to aid in navigating the robot <NUM> from a current location <NUM> in the environment <NUM> to the mission destination <NUM> in the environment. Here, the route specification <NUM> includes a series of four route segments <NUM>, 210a-d. The route segments <NUM> represent sequential portions of a path the robot <NUM> must traverse in order to successfully navigate to the mission destination <NUM>. In some examples, each route segment <NUM> in the series of route segments <NUM> includes a corresponding goal region <NUM>, 220a-d. Each goal region <NUM> represents an area that at least a portion of the robot <NUM> enters while traversing the corresponding respective route segment <NUM>. That is, in order to successfully "pass" or "succeed" the goal region <NUM> and move on to the next segment <NUM>, at least some portion of the robot <NUM> must enter the area designated by the goal region <NUM>. In some examples, the goal region <NUM> represents an area that a center point <NUM> of the robot <NUM> must enter while traversing the respective route segment <NUM>. Accordingly, the robot <NUM> is to sequentially traverse each route segment <NUM> in the series of route segments 210a-d by passing through each respective goal region <NUM> while navigating to the mission destination <NUM> in the environment <NUM>. In some examples, each route segment <NUM> in the series of route segments overlaps at least one other route segment <NUM> in the series of route segments <NUM>. In additional examples, each goal region <NUM> includes a convex shape in SE2 space. SE2 is a coordinate system that includes yaw with standard Cartesian coordinates (i.e., an x coordinate, a y coordinate, and yaw).

Optionally, each segment <NUM> includes a constraint region <NUM>, 230a-d that encompasses the respective goal region <NUM> of the segment <NUM>. The constraint region <NUM> establishes boundaries for the robot <NUM> to remain within while traversing toward the goal region <NUM> of the corresponding route segment <NUM>. That is, the constraint region <NUM> provides boundaries that allows the local navigation system <NUM> to freely navigate the robot <NUM> within while traversing the segment <NUM>. For example, in <FIG>, an obstacle <NUM> in the first route segment 210a impedes a direct path from the current location <NUM> of the robot <NUM> to the goal region 220a for the corresponding first route segment 210a. However, the constraint region 230a provides the local navigation system <NUM> with the context to understand that the robot <NUM> may safely navigate around the obstacle <NUM> using path <NUM> without failing the mission. In some examples, the constraint region <NUM> encompassing the goal region <NUM> for the corresponding route segment <NUM> also encompasses the goal region <NUM> associated with another one of the route segments <NUM> in the series of route segments <NUM> that overlaps the corresponding route segment <NUM>. For instance, in the example shown in <FIG>, the constraint region 230a encompassing the goal region 220a for the route segment 210a also encompasses the goal region 220b for the route segment 210b that overlaps the route segment 210a.

As another advantage, the local navigation system <NUM> may dynamically adjust an amount of margin <NUM> of distance between the robot <NUM> and obstacles <NUM> in response to the constraint region <NUM>. For example, when the constraint region <NUM> is large, the local navigation system <NUM> may provide additional margin <NUM> between the robot <NUM> and the obstacle <NUM> to provide for safer and smoother avoidance of the obstacle <NUM>. In other examples, when the constraint region <NUM> is small, the high-level navigation system <NUM> communicates to the local navigation system <NUM> that the mission requires a smaller margin <NUM> between the robot <NUM> and the obstacle <NUM> and therefore the local navigation system <NUM> will navigate the robot <NUM> closer to the obstacle <NUM> to remain within the constraint region <NUM>.

With continued reference to <FIG>, the exemplary route specification <NUM> includes four route segments 210a-d that the robot <NUM> must sequentially traverse when navigating to the mission destination <NUM>. For instance, the robot <NUM> traverses the first route segment 210a via the constraint region 230a to the corresponding goal region 220a adjacent to the bottom of a staircase <NUM> in the environment <NUM>. The route segment 210b provides the goal region 220b and the constraint region 230b to align the robot <NUM> with the staircase <NUM>. The route segment 210c and the constraint region 230c bound the robot <NUM> within the staircase <NUM> to allow the robot <NUM> to traverse up the staircase <NUM> to the corresponding goal region 220c adjacent to the top of the staircase <NUM>. Once the robot <NUM> reaches the top of the staircase <NUM> by entering the goal region 220c, the robot <NUM> traverses the fourth route segment 210d via the constrain region 230d to the goal region 220d (which, in this case, is also the mission destination <NUM>). Thus, in some examples, the goal region <NUM> for the last route segment <NUM> in the series of route segments encompasses the mission destination <NUM>.

Referring now to <FIG>, a route segment <NUM> specified by a route specification <NUM> may provide the local navigation system <NUM> with an initial path <NUM> for the robot <NUM> to follow while traversing the corresponding route segment <NUM>. The initial path <NUM> may be a "best guess" by the high-level navigation system <NUM> that does not take into account dynamic objects or obstacles that require local navigation to avoid. The local navigation system <NUM> may use the initial path <NUM> as a starting point for navigation, but deviate from the initial path <NUM> while remaining within the constraint region <NUM> as the environment <NUM> demands. In some examples, both goal regions <NUM> and constraint regions <NUM> are convex shapes to simplify problem solving for the navigation system <NUM>. A convex shape does not self-intersect and is a shape in which no line segment between two points on the boundary ever go outside of the shape.

Each route segment <NUM> may include any number of segment-specific constraints <NUM>. For example, each route segment <NUM> includes one or more of goal costs, velocity bounds, position constraints, position costs, velocity costs, yaw constraints/bounds, and/or mobility parameters. A cost may be associated with a "soft" constraint. That is, the robot <NUM> may violate a constraint with an associated cost under certain circumstances. The higher the cost, the harder the robot <NUM> (i.e., the navigation system <NUM>) will try to avoid violating the constraint. For example, a velocity cost may deter the navigation system <NUM> from exceeding a minimum velocity threshold and/or a maximum velocity threshold while traversing the corresponding route segment <NUM>. The velocity bounds may include angular velocity bounds, lateral velocity bounds, and longitudinal bounds. Likewise, position costs may encourage the robot <NUM> to maintain certain positions. Yaw constraints may impose yaw limitations on the robot <NUM>. For example, in narrow corridors or near ledges the robot <NUM> may be restricted from turning. Mobility parameters may categorize one or more surfaces covered by the corresponding route segment <NUM>. For example, the categorization includes stairs or flat ground. That is, a route segment <NUM> (e.g., the route segment 210c of <FIG>) that causes the robot <NUM> to traverse stairs may include a mobility parameter that categorizes the route segment <NUM> as stairs to allow the local navigation system <NUM> to react accordingly.

As the robot <NUM> navigates from one route segment <NUM> to a next route segment <NUM> specified by the route specification <NUM>, only the segment-specific constraints <NUM> associated with the route segment <NUM> that the robot <NUM> is currently traversing apply. This is opposed to global constraints <NUM> which the robot <NUM> must abide by no matter which route segment <NUM> the robot <NUM> is traversing. In some examples, the navigation system <NUM> includes only segment-specific constraints <NUM>, while in other examples, the navigation system <NUM> includes a hybrid of segment-specific constraints <NUM> and global constraints <NUM>. That is, the high-level navigation system <NUM> may impose both segment-specific constraints <NUM> and at least one global constraint <NUM> that constrains the robot <NUM> while traversing each segment <NUM>. For instance, world-based constraints (e.g., stairs) may be better modeled with global constraints <NUM>.

Referring now to <FIG>, another exemplary route specification <NUM> illustrates that the navigation system <NUM> does not use sequential composition. That is, the navigation system <NUM> may require the robot <NUM> to pass through each goal region <NUM> without skipping any goal region <NUM>. Here, the robot <NUM> must first pass through goal region 220e during route segment 210e, then while traversing route segment 210f, pass through goal region 220f, then while traversing route segment <NUM>, pass through goal region <NUM>, and so on and so forth until the robot reaches the mission destination <NUM> (not shown).

Referring now to <FIG>, in some implementations, the navigation system <NUM> generates the route specification <NUM> from a previously recorded map <NUM> (<FIG>). The previously recorded map <NUM> includes a plurality of waypoints <NUM>, 510a-n and a plurality of edges <NUM>, 520a-n generated by the robot <NUM> using sensor data <NUM> while previously traversing through the environment <NUM>. Each edge <NUM> connects a respective pair of the plurality of waypoints <NUM>. For example, the user <NUM> may command the robot <NUM> (via the user device <NUM>) to traverse the environment <NUM>, and as the robot moves about the environment, the robot <NUM> captures the sensor data <NUM> of the environment <NUM> for initial map generation and localization (e.g., simultaneous localization and mapping (SLAM)).

After the robot <NUM> traverses the environment <NUM> and generates the map <NUM>, the navigation system <NUM> receives a target destination (interchangeably referred to as "mission destination") <NUM> within the environment <NUM> for the robot <NUM> to navigate to. In some examples, the robot <NUM> receives a navigation request <NUM> (<FIG>) to navigate from the current location <NUM> in the environment <NUM> to the mission destination <NUM> in the environment <NUM>. The high-level navigation system <NUM> determines the route specification <NUM> which may include a series of waypoints <NUM> and corresponding edges <NUM> for the robot <NUM> to follow for navigating the robot <NUM> to the target destination <NUM>. Here, the series of waypoints <NUM> and corresponding edges <NUM> may be selected from the plurality of waypoints <NUM> and the plurality of edges <NUM> generated while the robot <NUM> previously traversed through the environment <NUM> during map generation. That is, the high-level navigation system <NUM> selects a subset of the plurality of waypoints <NUM> and associated edges <NUM> from the previously generated map <NUM> that best navigate the robot <NUM> to the mission destination <NUM>. In the example 500a of <FIG>, the high-level navigation system <NUM> selects waypoints 510a-c connected by edges 520a, 520b to navigate the robot to or near the mission destination <NUM>.

For each waypoint <NUM> in the series of waypoints <NUM> in the route specification <NUM>, the high-level navigation system <NUM> generates a goal region <NUM> encompassing the corresponding waypoint <NUM> in the route specification <NUM>. In the example 500b of <FIG>, the high-level navigation system <NUM> generates goal regions <NUM>-m encompassing the waypoints 510a-c. Specifically, the goal region <NUM> encompasses the waypoint 510a, the goal region <NUM> encompasses the waypoint 510b, and the goal region <NUM>. In some implementations, the high-level navigation system <NUM> passes additional context to the local navigation system <NUM> via a size of each goal region <NUM>. That is, when generating the goal region <NUM> encompassing the corresponding waypoint <NUM> in the route specification <NUM>, the high-level navigation system <NUM> generates a goal region <NUM> (i.e., a target region) encompassing the target destination <NUM> that includes a smaller area than at least one of the generated goal regions <NUM>. In some implementations, an area of the goal region <NUM> that encompasses the target destination <NUM> has an area that is smaller than each of the other goal regions.

For example, waypoints 510a, 510b are intermediate waypoints <NUM> on the way to the mission destination <NUM> and because there is no need for the robot <NUM> to precisely arrive at each waypoint <NUM>, the high-level navigation system <NUM> enlarges the size of the goal regions <NUM>, <NUM> to allow the local navigation system <NUM> more flexibility in passing each waypoint 510a, 510b. On the other hand, the waypoint 510c coincides at or near the mission destination <NUM> and here the high-level navigation system <NUM> has decreased the size of the goal region <NUM> to ensure that the local navigation system <NUM> navigates the robot <NUM> to the mission destination <NUM>. Because the local navigation system <NUM> must navigate a portion of the robot <NUM> (e.g., the center point of the robot) to a space in the environment <NUM> that is represented by the interior of the goal region <NUM>, the size of the goal region <NUM> establishes how near the waypoint <NUM> the robot <NUM> will get.

Each goal region <NUM> may include a yaw bound or yaw constraint that the robot must conform to before successfully satisfy or complete the respective goal region <NUM> and move on to the next segment <NUM>. That is, the yaw bound may force the robot <NUM> to enter a specific yaw configuration prior to leaving the goal region <NUM> before satisfying the goal region <NUM> and/or segment <NUM> while additionally or alternatively, the yaw bound forces the robot <NUM> to enter the specific yaw configuration before entering the goal region <NUM>. For example, when a goal region <NUM> is located at the bottom of a staircase (e.g., the goal region 220b of <FIG>), a yaw bound associated with the goal region <NUM> ensures that the robot <NUM> is properly aligned with the staircase prior to entering the goal region <NUM> and/or prior to leaving the goal region <NUM> and climbing the stairs. Thus, in some examples, one or more of the goal regions <NUM> are configured to cause the robot <NUM> to align with an obstacle prior to the robot <NUM> traversing the obstacle.

In some implementations, the high-level navigation system <NUM> generates at least one constraint region <NUM> encompassing at least one goal region <NUM>. The constraint region may encompass a plurality of goal regions <NUM> (e.g., two goal regions <NUM>). The at least one constraint region <NUM> establishes the boundaries for the robot <NUM> to remain within while traversing toward the target destination or mission destination <NUM>. In example 500c of <FIG>, the constraint region <NUM> encompasses the goal regions <NUM>, <NUM> while the constraint region <NUM> encompasses the goal regions <NUM>, <NUM>. The constraint regions <NUM>, <NUM> bound the areas the robot <NUM> may enter while the local navigation system <NUM> navigates the robot around obstacles to the mission destination <NUM>. Thus, the local navigation system <NUM> may navigate the robot <NUM> to the target destination or mission destination <NUM> by traversing the robot <NUM> through each goal region <NUM> (e.g., a center point of the robot <NUM>) while maintaining the robot <NUM> within the constraint regions <NUM>. The constraint region <NUM> may be any shape. Multiple goal regions <NUM> may overlap. In some examples, each segment <NUM> includes only a single constrained region <NUM> and a single goal region <NUM>.

Optionally, the high-level navigation system <NUM>, when generating the constraint regions <NUM>, generates each constraint region <NUM> such that the respective constraint region <NUM> is aligned with the corresponding edge <NUM> connecting the pair of waypoints <NUM> each encompassed by a goal region <NUM> and the constraint region <NUM>. In the example shown, the constraint region <NUM> is aligned with the edge 520a while the constraint region <NUM> is aligned with the edge 520b.

In some implementations, one or more of the goal regions <NUM> provide localization information to the robot <NUM>. For example, the underlying waypoint <NUM> encompassed by the goal region <NUM> from the previously generated map <NUM> allows the navigation system <NUM> to localize the robot <NUM> within the map <NUM>. In some examples, the high-level navigation system <NUM> sizes one or more constraint regions <NUM> to ensure that the robot <NUM> stays localized with the previously generated map <NUM>. That is, when the robot <NUM> strays too far away from waypoints <NUM> and edges <NUM> from the previously generated map, the navigation system <NUM> may lose track of the location of the robot within the environment <NUM>. To prevent this, the high-level navigation system <NUM> sizes the constraint region <NUM>, based on, for example, the capabilities of the vision system <NUM> and the fidelity of the map <NUM>, to keep the robot <NUM> localized within the map <NUM>. The constraint regions <NUM> may include additional contextual information for the local navigation system <NUM>, such as route cost information.

Referring now to <FIG>, in some examples, the high-level navigation system <NUM> solves for orientation constraints when generating the route specification <NUM>. In the example shown, an exemplary environment <NUM> includes a narrow corridor <NUM> that proceeds a staircase <NUM>. In some examples, it is beneficial for the robot <NUM> to traverse down stairs backwards to allow for additional space for the shins (e.g., lower portion <NUM> of each leg <NUM>) of the robot <NUM>. However, in this scenario, the narrow corridor <NUM> prohibits the robot <NUM> from turning around at the top of the staircase <NUM>. In this situation, the robot <NUM> must turn prior to entering the corridor <NUM> and traverse the corridor <NUM> backwards. Accordingly, at location <NUM>, the robot <NUM> executes a turning maneuver to re-orient itself backwards before entering the narrow corridor <NUM> so that the robot <NUM> may beneficially traverse down the staircase <NUM> backwards. By extending a constraint region <NUM> the entire length of the corridor <NUM>, the constraint region may enforce an orientation constraint on the robot <NUM>, thus commanding the robot <NUM> to turn around prior (e.g., at or near location <NUM>) to entering the corridor <NUM>.

Referring now to <FIG>, another exemplary environment <NUM> includes a staircase <NUM>. Here, an initial goal region 220n ensures that the robot <NUM> is aligned with the staircase <NUM> prior to traversing the stairs. Note that in this example, there is not a constraint region <NUM> constraining the path of the robot to the initial goal region 220n as the high-level navigation system <NUM> determined that a constraint region <NUM> was not necessary in this situation. A second goal region 220o along the stairs allows the local navigation system <NUM> to notify the high-level navigation system <NUM> when the robot is passing positions of good localization in the environment <NUM>. That is, in this example, the goal region 220o along the staircase <NUM> represents an area that allows the robot <NUM> to localize its location to the previously generated map <NUM>. A final goal region 220p ensures that the robot <NUM> is properly off the stairs prior to allowing the robot <NUM> to turn to proceed to the next segment <NUM> (not shown). The constraint regions <NUM> provide the robot <NUM> with position bounds and costs to keep the robot <NUM> centered on the stairs and to keep the yaw of the robot <NUM> aligned with the direction of the stairs.

<FIG> is a flowchart of an exemplary arrangement of operations for a method <NUM> of generating a route specification for navigating a robot <NUM>. At operation <NUM>, the method <NUM> includes receiving, at data processing hardware <NUM>, a navigation command <NUM> to navigate a robot <NUM> to a mission destination <NUM> within an environment <NUM> of the robot <NUM>. At operation <NUM>, the method <NUM> includes generating, by the data processing hardware <NUM>, a route specification <NUM> for navigating the robot <NUM> from a current location <NUM> in the environment <NUM> to the mission destination <NUM> in the environment <NUM>. The route specification <NUM> includes a series of route segments <NUM>.

Each route segment <NUM> in the series of route segments includes a goal region <NUM> for the corresponding route segment <NUM>, a constraint region <NUM> encompassing the goal region <NUM>, and an initial path <NUM> for the robot <NUM> to follow while traversing the corresponding route segment <NUM>. The constraint region <NUM> encompassing the goal region <NUM> for each corresponding route segment <NUM> establishes boundaries for the robot <NUM> to remain within while traversing toward the goal region <NUM>.

For example, it may be implemented as a standard server 900a or multiple times in a group of such servers 900a, as a laptop computer 900b, or as part of a rack server system 900c.

Claim 1:
A method (<NUM>) comprising:
receiving, at data processing hardware (<NUM>), a navigation command (<NUM>) to navigate a robot (<NUM>) to a mission destination (<NUM>) within an environment (<NUM>) of the robot (<NUM>);
generating, by the data processing hardware (<NUM>), a route specification (<NUM>) for navigating the robot (<NUM>) from a current location (<NUM>) in the environment (<NUM>) to the mission destination (<NUM>) in the environment (<NUM>), the route specification (<NUM>) comprising a series of route segments (<NUM>), each route segment (<NUM>) in the series of route segments (<NUM>) comprising:
a goal region (<NUM>) for the corresponding route segment (<NUM>);
a constraint region (<NUM>) encompassing the goal region (<NUM>), the constraint region (<NUM>) establishing boundaries for the robot (<NUM>) to remain within while traversing toward the goal region (<NUM>) and avoiding any obstacles (<NUM>) within the constraint region (<NUM>); and
an initial path (<NUM>) for the robot (<NUM>) to follow while traversing the corresponding route segment (<NUM>);
characterized in:
adjusting, by the data processing hardware (<NUM>), an amount of margin (<NUM>) of distance between the robot (<NUM>) and the obstacles (<NUM>) within the constraint region (<NUM>) based on a size of the constraint region (<NUM>).