Patent Description:
This disclosure relates to alternate route finding for navigation maps.

Robotic devices navigate constrained environments to perform a variety of tasks or functions. To navigate through these constrained environments, the robotic devices may identify obstacles. Based on the identification of the obstacles, the robotic devices can navigate the constrained environment without contacting the obstacles.

<CIT> discloses a method for generating intermediate waypoints for a navigation system of a robot including receiving a navigation route. The navigation route includes a series of high-level waypoints that begin at a starting location and end at a destination location and is based on high-level navigation data. The high-level navigation data is representative of locations of static obstacles in an area the robot is to navigate. The method also includes receiving image data of an environment about the robot from an image sensor and generating at least one intermediate waypoint based on the image data. The method also includes 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 toward the destination location.

<CIT> discloses a method, system, and logic where planning a journey from an origin to a destination involves establishing selection criteria that includes one or more journey parameters and one or more rules. A journey parameter describes a constraint of the journey, and a rule specifies an action to perform in response to a trigger event. A route for the journey from the origin to the destination is planned. A trigger event is detected after initiation of the journey. A rule specifying an action to perform in response to the detected trigger event is accessed. One or more alternate waypoints are selected in accordance with the action and the selection criteria. The one or more alternate waypoints are inserted into the route to create a next route.

<CIT> discloses a method and system where information about selection parameters is obtained from a starting position to a destination position along an initial route. At least one alternative route is calculated by selecting a waypoint as an intermediate destination based on an initial route calculation and the selection parameters. The at least one alternative route is determined through the waypoint differs from the initial route on a specific section on which the intermediate destination lies.

An aspect of the present disclosure provides a computer-implemented method that when executed by data processing hardware of a robot causes the data processing hardware to perform operations as defined by the appended independent claim <NUM>. The operations include obtaining a topological map including a series of route waypoints representative of a navigation route for the robot to follow from a start location to a destination location. The operations include receiving image data of an environment of the robot from an image sensor. The operations include determining, using the image data, that a route edge that connects a first route waypoint of the series of route waypoints to a second route waypoint of the series of route waypoints is at least partially blocked by an obstacle. The operations include generating, using the image data and the topological map, an alternate waypoint and an alternate edge connecting the alternate waypoint to the first route waypoint. The operations include adjusting the navigation route to include the alternate waypoint and the alternate edge, the alternate waypoint and the alternate edge bypassing the route edge that is at least partially blocked by the obstacle.

In some implementations, the operations include navigating the robot from the first route waypoint to the alternate waypoint by traversing the robot along the alternate edge connecting the alternate waypoint to the first route waypoint.

In some implementations, adjusting the navigation route includes obtaining a cost for each route edge of a series of route edges of the topological map and each alternate edge of one or more alternate edges and determining a route from a current location of the robot to the destination location using one or more route edges and/or alternate edges with an aggregate cost less than a cost threshold value. In some embodiments, route edges that are not at least partially blocked by one or more obstacles may have a cost less than a cost of alternate edges. In other embodiments, route edges that are at least partially blocked by one or more obstacles may have a cost greater than or equal to the cost of alternate edges. In some examples, the alternate waypoint is offset from at least one route waypoint of the series of route waypoints by an offset distance. The offset distance may be less than or equal to an operational range of the image sensor.

In some implementations, the operations further include generating a second alternate edge connecting the alternate waypoint and a second alternate waypoint. In some examples, the operations further include, determining that the second alternate edge connecting the alternate waypoint and the second alternate waypoint is blocked, and readjusting the navigation route to include a third alternate waypoint. The readjusted navigation route may bypass the second alternate edge.

In some embodiments, generating the alternate waypoint includes using a polyline buffer algorithm to generate the alternate waypoint. In some examples, generating the alternate waypoint includes generating one or more alternate waypoints within a threshold distance of a current location of the robot.

In some implementations, the topological map indicates one or more locations of one or more static obstacles within the environment. In some cases, the topological map indicates a location of the alternate waypoint is free from static obstacles.

In some examples, generating the alternate waypoint includes generating at least two alternate waypoints, and the at least two alternate waypoints are connected by a respective alternate edge.

Another aspect of the present disclosure provides a system. The system includes data processing hardware of a robot 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 as defined by the appended independent claim <NUM>. The operations include obtaining a topological map including a series of route waypoints representative of a navigation route for the robot to follow from a start location to a destination location. The operations include receiving image data of an environment of the robot from an image sensor. The operations include determining, using the image data, that a route edge that connects a first route waypoint of the series of route waypoints to a second route waypoint of the series of route waypoints is at least partially blocked by an obstacle. The operations include generating, using the image data and the topological map, an alternate waypoint and an alternate edge connecting the alternate waypoint to the first route waypoint. The operations include adjusting the navigation route to include the alternate waypoint and the alternate edge, the alternate waypoint and the alternate edge bypassing the route edge that is at least partially blocked by the obstacle.

In some implementations, adjusting the navigation route includes obtaining a cost for each route edge of a series of route edges of the topological map and each alternate edge of one or more alternate edges and determining a route from a current location of the robot to the destination location using one or more route edges and/or alternate edges with an aggregate cost less than a cost threshold value. In some embodiments, route edges that are not at least partially blocked by one or more obstacles may have a cost less than a cost of alternate edges. In other embodiments, route edges that are at least partially blocked by one or more obstacles may have a cost greater than or equal the cost of alternate edges. In some examples, the alternate waypoint is offset from at least one route waypoint of the series of route waypoints by an offset distance. The offset distance may be less than or equal to an operational range of the image sensor.

The details of the one or more implementations of the disclosure are set forth in the accompanying drawings and the description below.

Autonomous and semi-autonomous robots may be equipped with complex mapping, localization, and navigation systems. These systems may utilize navigation graphs. The navigation graph may include an obstacle map for local navigation. However, the obstacle map for local navigation may be limited in size, complexity, the information provided by the obstacle map, etc. For example, the obstacle map may be limited based on a sensing range threshold (e.g., a sensing range threshold of the robot) and/or to increase efficiency, reduce computational cost, etc. Further, the obstacle map may be limited to limit the robot to exploring a particular known safe area around a recorded path of the obstacle map and/or to prevent oscillating interactions by the robot with the obstacle map. Therefore, the obstacle map may lead to the robot becoming "stuck" on obstacles due to the obstacle map.

Implementations herein are directed toward systems and methods for navigating a robot with additional flexibility for exploration in a verifiably-safe manner. The navigation system enables a robot to reroute (e.g., deterministically) around a plurality of classes of obstacles using an automated post-processing algorithm that adds graph structure in particular areas (e.g., areas where a predicted traversability of the area exceeds or matches a traversability threshold and/or areas where an observed portion of the area exceeds or matches an observed threshold). The robot may identify one or more additional paths based on the added graph structure that the robot may take to avoid a blockage within the nominal path. After avoiding the blockage, the robot may return to the nominal path. Therefore, the robot can gain additional navigation flexibility during runtime using the added graph structure in a deterministic manner and the added graph structure can be tuned and verified offline.

Referring to <FIG> and <FIG>, in some implementations, a robot <NUM> includes a body <NUM> with one or more locomotion-based structures such as legs 120a-d coupled to the body <NUM> that enable the robot <NUM> to move within an environment <NUM> that surrounds the robot <NUM>. In some examples, all or a portion of the legs <NUM> are an articulable structure such that one or more joints J permit members <NUM> of the leg <NUM> to move. For instance, in the illustrated embodiment, all or a portion of the legs <NUM> include a hip joint JH coupling an upper member <NUM>, 122U of the leg <NUM> to the body <NUM> and a knee joint JK coupling the upper member 122U of the leg <NUM> to a lower member <NUM> of the leg <NUM>. Although <FIG> depicts a quadruped robot with four legs 120a-d, the robot <NUM> may include any number of legs or locomotive based structures (e.g., a biped or humanoid robot with two legs, or other arrangements of one or more legs) that provide a means to traverse the terrain within the environment <NUM>.

In order to traverse the terrain, all or a portion of the legs <NUM> may have a distal end <NUM> that contacts a surface of the terrain (e.g., a traction surface). Further, the distal end <NUM> of the leg <NUM> may be the end of the leg <NUM> used by the robot <NUM> to pivot, plant, or generally provide traction during movement of the robot <NUM>. For example, the distal end <NUM> of a leg <NUM> corresponds to a foot of the robot <NUM>. In some examples, though not shown, the distal end of the leg includes an ankle joint such that the distal end is articulable with respect to the lower member of the leg.

In the examples shown, the robot <NUM> includes an arm <NUM> that functions as a robotic manipulator. The arm <NUM> may move about multiple degrees of freedom in order to engage elements of the environment <NUM> (e.g., objects within the environment <NUM>). In some examples, the arm <NUM> includes one or more members <NUM>, where the members <NUM> are coupled by joints J such that the arm <NUM> may pivot or rotate about the joint(s) J. For instance, with more than one member <NUM>, the arm <NUM> may extend or to retract. To illustrate an example, <FIG> depicts the arm <NUM> with three members <NUM> corresponding to a lower member <NUM>L, an upper member <NUM>U, and a hand member <NUM>H (also referred to as an end-effector). Here, the lower member <NUM>L may rotate or pivot about a first arm joint JA1 located adjacent to the body <NUM> (e.g., where the arm <NUM> connects to the body <NUM> of the robot <NUM>). The lower member <NUM>L is coupled to the upper member <NUM>U at a second arm joint JA2 and the upper member <NUM>U is coupled to the hand member <NUM>H at a third arm joint JA3. In some examples, such as <FIG>, the hand member <NUM>H is a mechanical gripper that includes a moveable jaw and a fixed jaw that perform different types of grasping of elements within the environment <NUM>. In the example shown, the hand member <NUM>H includes a fixed first jaw and a moveable second jaw that grasps objects by clamping the object between the jaws. The moveable jaw can move relative to the fixed jaw to move between an open position for the gripper and a closed position for the gripper (e.g., closed around an object). In some implementations, the arm <NUM> additionally includes a fourth joint JA4. The fourth joint JA4 may be located near the coupling of the lower member <NUM>L to the upper member <NUM>U and function to allow the upper member 128u to twist or rotate relative to the lower member <NUM>L. Therefore, the fourth joint JA4 may function as a twist joint similarly to the third joint JA3 or wrist joint of the arm <NUM> adjacent the hand member <NUM>H. For instance, as a twist joint, one member coupled at the joint J may move or rotate relative to another member coupled at the joint J (e.g., a first member coupled at the twist joint is fixed while the second member coupled at the twist joint rotates). In some implementations, the arm <NUM> connects to the robot <NUM> at a socket on the body <NUM> of the robot <NUM>. In some configurations, the socket is configured as a connector such that the arm <NUM> attaches or detaches from the robot <NUM> depending on whether the arm <NUM> is desired for particular operations.

The robot <NUM> has a vertical gravitational axis (e.g., shown as a Z-direction axis AZ) along a direction of gravity, and a center of mass CM, which is a position that corresponds to an average position of all parts of the robot <NUM> where the parts are weighted according to their masses (e.g., 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 AZ (e.g., 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> (e.g., the combination of the position of the CM of the robot and the attitude or orientation of the robot <NUM>). Here, a height generally refers to a distance along the z-direction (e.g., along a z-direction axis AZ). The sagittal plane of the robot <NUM> corresponds to the Y-Z plane extending in directions of a y-direction axis AY and the z-direction axis AZ. Therefore, the sagittal plane bisects the robot <NUM> into a left and a right side. Generally perpendicular to the sagittal plane, a ground plane (also referred to as a transverse plane) spans the X-Y plane by extending in directions of the x-direction axis AX and the y-direction axis AY. The ground plane refers to a ground surface <NUM> where distal ends <NUM> of the legs <NUM> of the robot <NUM> may generate traction to help the robot <NUM> move within the environment <NUM>. Another anatomical plane of the robot <NUM> is the frontal plane that extends across the body <NUM> of the robot <NUM> (e.g., from a right side of the robot <NUM> with a first leg 120a to a left side of the robot <NUM> with a second leg 120b). The frontal plane spans the X-Z plane by extending in directions of the x-direction axis AX and the z-direction axis Az.

In order to maneuver within the environment <NUM> or to perform tasks using the arm <NUM>, the robot <NUM> includes a sensor system <NUM> (also referred to as a vision system) with one or more sensors <NUM>, 132a-n. For example, <FIG> illustrates a first sensor <NUM>, 132a mounted at a head of the robot <NUM> (e.g., near a front portion of the robot <NUM> adjacent the front legs 120a-b), a second sensor <NUM>, 132b mounted near the hip of the second leg 120b of the robot <NUM>, a third sensor <NUM>, 132c corresponding to one of the sensors <NUM> mounted on a side of the body <NUM> of the robot <NUM>, a fourth sensor <NUM>, 132d mounted near the hip of the fourth leg 120d of the robot <NUM>, and a fifth sensor <NUM>, 132e mounted at or near the hand member <NUM>H of the arm <NUM> of the robot <NUM>. The sensors <NUM> may include vision/image sensors, inertial sensors (e.g., an inertial measurement unit (IMU)), force sensors, and/or kinematic sensors. For example, the sensors <NUM> may include one or more of an image sensor, (e.g., a camera, a stereo camera, etc.), a time-of-flight (TOF) sensor, a scanning light-detection and ranging (LIDAR) sensor, or a scanning laser-detection and ranging (LADAR) sensor. In some examples, the sensor <NUM> has a corresponding field(s) of view Fv defining a sensing range or region corresponding to the sensor <NUM>. For instance, <FIG> depicts a field of a view FV for the first sensor <NUM>, 132a of the robot <NUM>. Each sensor <NUM> may be pivotable and/or rotatable such that the sensor <NUM>, for example, changes the field of view FV about one or more axis (e.g., an x-axis, a y-axis, or a z-axis in relation to a ground plane). In some examples, multiple sensors <NUM> may be clustered together (e.g., similar to the first sensor 132a) to stitch a larger field of view FV than any single sensor <NUM>. With multiple sensors <NUM> placed about the robot <NUM>, the sensor system <NUM> may have a <NUM> degree view or a nearly <NUM> degree view of the surroundings of the robot <NUM> about vertical or horizontal axes.

When surveying a field of view FV with a sensor <NUM> (see e.g., <FIG>), the sensor system <NUM> generates sensor data <NUM> (e.g., image data) corresponding to the field of view FV. The sensor system <NUM> may generate the field of view FV with a sensor <NUM> mounted on or near the body <NUM> of the robot <NUM> (e.g., sensor(s) 132a, 132b). The sensor system may additionally and/or alternatively generate the field of view FV with a sensor <NUM> mounted at or near the hand member <NUM>H of the arm <NUM> (e.g., sensor(s) 132c). The one or more sensors <NUM> capture the sensor data <NUM> that defines the three-dimensional point cloud for the area within the environment <NUM> of the robot <NUM>. In some examples, the sensor data <NUM> is image data that corresponds to a three-dimensional volumetric point cloud generated by a three-dimensional volumetric image sensor <NUM>. Additionally or alternatively, when the robot <NUM> is maneuvering within the environment <NUM>, the sensor system <NUM> gathers pose data for the robot <NUM> that includes inertial measurement data (e.g., measured by an IMU). In some examples, the pose data includes kinematic data and/or orientation data about the robot <NUM>, for instance, kinematic data and/or orientation data about joints J or other portions of a leg <NUM> or arm <NUM> of the robot <NUM>. With the sensor data <NUM>, various systems of the robot <NUM> may use the sensor data <NUM> to define a current state of the robot <NUM> (e.g., of the kinematics of the robot <NUM>) and/or a current state of the environment <NUM> of the robot <NUM>. Therefore, the sensor system <NUM> may communicate the sensor data <NUM> from one or more sensors <NUM> to any other system of the robot <NUM> in order to assist the functionality of that system.

In some implementations, the sensor system <NUM> includes sensor(s) <NUM> coupled to a joint J. Moreover, these sensors <NUM> may couple to a motor M that operates a joint J of the robot <NUM> (e.g., sensors <NUM>). Here, these sensors <NUM> generate joint dynamics in the form of joint-based sensor data <NUM>. Joint dynamics collected as joint-based sensor data <NUM> may include joint angles (e.g., an upper member <NUM>U relative to a lower member <NUM>L or hand member <NUM>H relative to another member of the arm <NUM> or robot <NUM>), joint speed joint angular velocity, joint angular acceleration, and/or forces experienced at a joint J (also referred to as joint forces). Joint-based sensor data generated by one or more sensors <NUM> may be raw sensor data, data that is further processed to form different types of joint dynamics, or some combination of both. For instance, a sensor <NUM> measures joint position (or a position of member(s) <NUM> or <NUM> coupled at a joint J) and systems of the robot <NUM> perform further processing to derive velocity and/or acceleration from the positional data. In other examples, a sensor <NUM> can measure velocity and/or acceleration directly.

With reference to <FIG>, as the sensor system <NUM> gathers sensor data <NUM>, a computing system <NUM> stores, processes, and/or communicates the sensor data <NUM> to various systems of the robot <NUM> (e.g., the control system <NUM>, a navigation system <NUM>, and/or remote controller <NUM>). In order to perform computing tasks related to the sensor data <NUM>, the computing system <NUM> of the robot <NUM> includes data processing hardware <NUM> (e.g., a hardware processor) and memory hardware <NUM> (e.g., a memory circuit). The data processing hardware <NUM> can execute instructions stored in the memory hardware <NUM> to perform computing tasks related to activities (e.g., movement and/or movement based activities) for the robot <NUM>. The computing system <NUM> may refer to one or more locations of data processing hardware <NUM> and/or memory hardware <NUM>.

In some examples, the computing system <NUM> is a local system located on the robot <NUM>. When located on the robot <NUM>, the computing system <NUM> may be centralized (e.g., in a single location/area on the robot <NUM>, for example, the body <NUM> of the robot <NUM>), decentralized (e.g., located at various locations about the robot <NUM>), or a hybrid combination of both (e.g., including centralized hardware and decentralized hardware). To illustrate some differences, a decentralized computing system <NUM> may allow processing to occur at an activity location (e.g., at a motor that moves a joint of a leg <NUM>) while a centralized computing system <NUM> may allow for a central processing hub that communicates to systems located at various positions on the robot <NUM> (e.g., communicate to the motor that moves the joint of the leg <NUM>).

Additionally or alternatively, the computing system <NUM> includes computing resources that are located remote from the robot <NUM>. For instance, the computing system <NUM> communicates via a network <NUM> with a remote system <NUM> (e.g., a remote server or a cloud-based environment). Much like the computing system <NUM>, the remote system <NUM> includes remote computing resources such as remote data processing hardware <NUM> and remote memory hardware <NUM>. Sensor data <NUM> or other processed data (e.g., data processing locally by the computing system <NUM>) may be stored in the remote system <NUM> and may be accessible to the computing system <NUM>. In additional examples, the computing system <NUM> can utilize the remote resources <NUM>, <NUM> as extensions of the computing resources <NUM>, <NUM> such that resources of the computing system <NUM> reside on resources of the remote system <NUM>. In some examples, the remote system <NUM>, or at least a portion of the remote resources <NUM>, <NUM> reside on a remote controller <NUM> in communication with the robot <NUM>.

In some implementations, as shown in <FIG>, the robot <NUM> includes a control system <NUM>. The control system <NUM> may communicate with systems of the robot <NUM>, such as the at least one sensor system <NUM> and the navigation system <NUM>. The control system <NUM> may perform operations and other functions using hardware <NUM>. The control system <NUM> includes at least one controller <NUM> that can control the robot <NUM>. For example, the controller <NUM> controls movement of the robot <NUM> to about the environment <NUM> based on input or feedback from the systems of the robot <NUM> (e.g., the sensor system <NUM> and/or the control system <NUM>). In additional examples, the controller <NUM> controls movement between poses and/or behaviors of the robot <NUM>. The at least one controller <NUM> may control movement of the arm <NUM> of the robot <NUM> in order for the arm <NUM> to perform various tasks using the hand member <NUM>H. For instance, the at least one controller <NUM> may control the hand member <NUM>H (e.g., a gripper) to manipulate an object or element in the environment <NUM>. For example, the controller <NUM> actuates the movable jaw in a direction towards the fixed jaw to close the gripper. In other examples, the controller <NUM> actuates the movable jaw in a direction away from the fixed jaw to close the gripper.

A given controller <NUM> of the control system <NUM> may control the robot <NUM> by controlling movement about one or more joints J of the robot <NUM>. In some configurations, the given controller <NUM> includes software or firmware with programming logic that controls at least one joint J or a motor M which operates, or is coupled to, a joint J. A software application (a software resource) may refer to computer software that causes a computing device to perform a task. " For instance, the controller <NUM> may control an amount of force that is applied to a joint J (e.g., torque at a joint J). As programmable controllers <NUM>, the number of joints J that a controller <NUM> controls is scalable and/or customizable for a particular control purpose. A controller <NUM> may control a single joint J (e.g., control a torque at a single joint J), multiple joints J, or actuation of one or more members <NUM> (e.g., actuation of the hand member <NUM>H) of the robot <NUM>. By controlling one or more joints J, actuators or motors M, the controller <NUM> may coordinate movement for all different parts of the robot <NUM> (e.g., the body <NUM>, one or more legs <NUM>, the arm <NUM>). For example, to perform a behavior with some movements, a controller <NUM> may control movement of multiple parts of the robot <NUM> such as, for example, two legs 120a-b, four legs 120a-d, or two legs 120a-b combined with the arm <NUM>. In some examples, the controller <NUM> is an object-based controller that is set up to perform a particular behavior or set of behaviors for interacting with an interactable object.

With continued reference to <FIG>, an operator <NUM> (also referred to herein as a user or a client) may interact with the robot <NUM> via the remote controller <NUM> that communicates with the robot <NUM> to perform actions. For example, the operator <NUM> transmits commands <NUM> to the robot <NUM> (executed via the control system <NUM>) via a wireless communication network <NUM>. Additionally, the robot <NUM> may communicate with the remote controller <NUM> to display an image on a user interface <NUM> (e.g., UI <NUM>) of the remote controller <NUM>. For example, the UI <NUM> can display the image that corresponds to a three-dimensional field of view Fv of the one or more sensors <NUM>. The image displayed on the UI <NUM> of the remote controller <NUM> may be a two-dimensional image that corresponds to the three-dimensional point cloud of sensor data <NUM> (e.g., field of view Fv) for the area within the environment <NUM> of the robot <NUM>. That is, the image displayed on the UI <NUM> may be a two-dimensional image representation that corresponds to the three-dimensional field of view Fv of the one or more sensors <NUM>.

Referring now to <FIG>, the robot <NUM> (e.g., the data processing hardware <NUM>) executes the navigation system <NUM> for enabling the robot <NUM> to navigate the environment <NUM>. The sensor system <NUM> includes one or more imaging sensors <NUM> (e.g., image sensors, LIDAR sensors, LADAR sensors, etc.) that can each capture sensor data <NUM> of the environment <NUM> surrounding the robot <NUM> within the field of view FV. For example, the one or more sensors <NUM> may be one or more cameras. The sensor system <NUM> may move the field of view FV by adjusting an angle of view or by panning and/or tilting (either independently or via the robot <NUM>) one or more sensors <NUM> to move the field of view FV in any direction. In some implementations, the sensor system <NUM> includes multiple sensors <NUM> (e.g., multiple cameras) such that the sensor system <NUM> captures a generally <NUM>-degree field of view around the robot <NUM>.

In the example of <FIG>, the navigation system <NUM> includes a high-level navigation module <NUM> that receives map data <NUM> (e.g., high-level navigation data representative of locations of static obstacles in an area the robot <NUM> is to navigate). In some cases, the map data <NUM> includes a graph map <NUM>. In other cases, the high-level navigation module <NUM> generates the graph map <NUM>. The graph map <NUM> may include a topological map of a given area the robot <NUM> is to traverse. The high-level navigation module <NUM> can obtain (e.g., from the remote system <NUM> or the remote controller <NUM>) and/or generate a series of route waypoints 310a, 310b (as shown in <FIG> and <FIG>) on the graph map <NUM> for a navigation route <NUM> that plots a path around large and/or static obstacles from a start location (e.g., the current location of the robot <NUM>) to a destination. Route edges may connect corresponding pairs of adjacent route waypoints. In some examples, the route edges record geometric transforms between route waypoints based on odometry data. The route waypoints and the route edges may be representative of the navigation route <NUM> for the robot <NUM> to follow from a start location to a destination location.

In some examples, the high-level navigation module <NUM> produces the navigation route <NUM> over a greater than <NUM> meter scale (e.g., the navigation route <NUM> may include distances greater than <NUM> meters from the robot <NUM>). The navigation system <NUM> also includes a local navigation module <NUM> that can receive the navigation route <NUM> and the image or sensor data <NUM> from the sensor system <NUM>. The local navigation module <NUM>, using the sensor data <NUM>, can generate an obstacle map <NUM>. The obstacle map <NUM> may be a robot-centered map that maps obstacles (static and/or dynamic obstacles) in the vicinity (e.g., within a threshold distance) of the robot <NUM> based on the sensor data <NUM>. For example, while the graph map <NUM> may include information relating to the locations of walls of a hallway, the obstacle map <NUM> (populated by the sensor data <NUM> as the robot <NUM> traverses the environment <NUM>) may include information regarding a stack of boxes placed in the hallway that were not present during the original recording. The size of the obstacle map <NUM> may be dependent upon both the operational range of the sensors <NUM> (e.g., a camera, LIDAR, etc.) and the available computational resources.

The local navigation module <NUM> can generate a step plan <NUM> (e.g., using an A* search algorithm) that plots all or a portion of the individual steps (or other movement) of the robot <NUM> to navigate from the current location of the robot <NUM> to the next waypoint along the navigation route <NUM>. Using the step plan <NUM>, the robot <NUM> can maneuver through the environment <NUM>. The local navigation module <NUM> may obtain a path for the robot <NUM> to the next waypoint using an obstacle grid map based on the captured sensor data <NUM>. In some examples, the local navigation module <NUM> operates on a range correlated with the operational range of the sensor <NUM> (e.g., four meters) that is generally less than the scale of high-level navigation module <NUM>.

Referring now to <FIG> and <FIG>, in some examples, the navigation system imposes one or more constraints on the robot <NUM> as the robot <NUM> attempts to traverse a navigation route. For example, the robot <NUM> may be constrained from deviating more than a threshold distance from the navigation route and/or route waypoints 310a, b or route edges <NUM> that make up the navigation route. When the obstacle map is smaller than a particular threshold value (e.g., to reduce computational cost or due to sensor range), the navigation system may impose constraints to limit the robot's exploration during navigation to known safe areas near the navigation route and to, for example, prevent oscillating interactions with the small obstacle map size. However, these constraints can limit the robot's ability to navigate around obstacles that impede or block the navigation route.

A schematic view 300a illustrates a location of the robot <NUM> on the graph map <NUM>. The local navigation module of the robot may impose a boundary constraint <NUM> on exploration of the robot <NUM> as the robot attempts to navigate from a first route waypoint 310a to a second route waypoint 310b along an edge <NUM>. The size and location of the boundary constraint <NUM> may be based on the location of the route waypoints 310a, b, the sensor range of the robot's sensors, the computational resources of the robot <NUM>, and/or the size and quality of the graph map <NUM>. In the example of <FIG>, the robot <NUM>, while navigating toward the waypoint 310b, encounters an obstacle <NUM>. As shown in <FIG>, the obstacle <NUM> extends from the left side of the boundary box <NUM> to the right side of the boundary box <NUM>. The robot may become stuck based on the position of the obstacle <NUM>. For example, because the robot <NUM> cannot see a path around the obstacle <NUM> without violating the constraint imposed by the bounding constraint <NUM>, the robot may become stuck. Referring now to <FIG>, a schematic view 300b illustrates that, in this example, that, after manually driving the robot <NUM> to the left, there is ample room to bypass the obstacle <NUM>. However, due to constraints (e.g., the boundary constraint <NUM>), the robot <NUM> may be unable to bypass the obstacle <NUM> autonomously.

Referring now to <FIG>, in some examples, the navigation system <NUM> includes an alternate waypoint generator <NUM>. The local navigation module <NUM> may obtain the sensor data <NUM> (e.g., image data) of the environment of the robot <NUM>. The local navigation module <NUM> may determine, using the sensor data <NUM>, that a corresponding one of the series of route edges 312a that connects the first route waypoint 310a and the second route waypoint 310b is blocked by an obstacle <NUM>. For example, the local navigation module <NUM> may determine that the route edge is blocked, as the robot <NUM> navigates along the navigation route <NUM> from a first route waypoint 310a to a second route waypoint 310b. In the example of <FIG>, as the robot <NUM> navigates the navigation route <NUM> by traversing along the route edge 312a that connects the route waypoint 310a and the route waypoint 310b, the robot <NUM> encounters the obstacle <NUM> that blocks the route edge 312a. Because the obstacle <NUM> intersects the width of the boundary constraint <NUM> (which may correspond to a size of the obstacle map <NUM>), the robot <NUM> may be unable to determine a route around the obstacle <NUM> and may remain within the boundary constraint <NUM>.

Referring now to <FIG>, the alternate waypoint generator <NUM> may receive the sensor data <NUM> (or a derivative of the sensor data <NUM> such as the obstacle map <NUM>) and the graph map <NUM> with the navigation route 212A. The alternate waypoint generator <NUM> can generate, using the sensor data <NUM> and the graph map <NUM>, one or more alternate waypoints 310Aa, b each offset from one of the route waypoints 310a, b by an offset distance <NUM>. The offset distance <NUM> may be correlated with an operations range of the sensor(s) (e.g., the offset distance is less than the operational range of the sensor(s), such as an image sensor, LIDAR sensor, LADAR sensor, etc.). A user computing device may provide information indicating the offset distance. Further, the offset distance <NUM> may be based on a number of factors, including the environment the robot <NUM> is traversing. In some implementations, the alternate waypoint generator <NUM> may use sensor data <NUM> from one or more sensors that is different than the original sensor(s) <NUM> used to capture sensor data <NUM> associated with the obstacle <NUM> and/or the boundary constraint <NUM>. For example, the robot <NUM> may primarily operate on image data captured by a camera to reduce computational resources. However, the alternate waypoint generator <NUM> may process longer-range data (e.g., captured by a different sensor such as a LIDAR sensor) that is more computationally expensive than the image sensor so that the alternate waypoint generator <NUM> may generate alternate waypoints 310Aa, b and/or alternate edges 312Aa, b, c further from the original route 212A (when there are no obstacles <NUM>) than the alternate waypoint generator <NUM> otherwise would be able to with the sensor data <NUM>.

For all or a portion of the alternate waypoints 310Aa, b, the alternate waypoint generator <NUM> may generate a respective alternate edge 312Aa, b, c connecting the alternate waypoint 310Aa, b to a respective route waypoint 310a, b. In some examples, the alternate waypoint generator <NUM> determines a correspondence between all or a portion of the alternate waypoints 310Aa, b and a route waypoint 310a, b. For all or a portion of the correspondence, if the robot <NUM> determines that the traversability of the straight-line region from the alternate waypoint 310Aa, b to the corresponding route waypoint 310a, b is greater than a threshold value (e.g., the robot <NUM> determines that the robot can traverse the straight-line region), the alternate waypoint generator <NUM> generates the alternate edge 312Aa, b, c and confirms the alternate waypoint 310Aa, b. In some implementations, when the robot <NUM> determines that the traversability of the straight-line region from the alternate waypoint 310Aa, b to the corresponding route waypoint 310a, b is below a particular threshold value, the alternate waypoint generator <NUM> may adjust the location of the alternate waypoint 310Aa, b (e.g., within a threshold distance of the original location) and may evaluate the path again.

In the example of <FIG>, the alternate waypoint generator <NUM> may generate alternate waypoints 310Aa and 310Ab. The alternate waypoint 310Aa is connected to the route waypoint 310a via the alternate edge 312Aa and the alternate waypoint 310Ab is connected to the route waypoint 310b via the alternate edge 312Ac. Further, the alternate waypoints 310Aa, 310Ab are connected via the alternate edge 312Ab.

The alternate waypoint generator <NUM> may generate alternate edges 312Aa, b, c between pairs of alternate waypoints 310Aa, b when all or a portion of the alternate waypoints 310Aa, b satisfy one of the following criteria. A first criteria is satisfied when all or a portion of the alternate waypoints 310Aa, b connect to the same route waypoint 310a, b. A second criteria is satisfied when all or a portion of the alternate waypoints 310Aa, b connect to an end of a single route edge <NUM>. A third criteria is satisfied when all or a portion of the alternate waypoints 310Aa, b connect to one of two route waypoints 310a, b that have a particular path length between the two route waypoints 310a, b (e.g., a path length with a length below or equal to a particular threshold value, a normal path length, etc.). If the third criteria is satisfied, as discussed in more detail below, the alternate waypoint generator <NUM> may increase a cost of the corresponding alternate edge 312Aa, b, c to maintain a preference for the original navigation route <NUM>. The alternate waypoint generator <NUM> may perform all or a portion of the traversability checks performed on the route edge <NUM> on all or a portion of the alternate edges <NUM>, 312A. Thus, the alternate waypoint generator <NUM> may minimize the extent to which the region of allowed space around any alternate edge 312Aa, b, c duplicates the region of allowed space around any route edge <NUM>. Therefore, the alternate waypoint generator <NUM> can minimize the overlap of the boundary constraint <NUM> and any new boundary constraints established by the alternate edges 312Aa, b, c based on sensor and/or computation constraints. As detecting that an edge is not traversable at playback time is time consuming and has an increased risk of the robot <NUM> falling while trying to traverse a blocked and/or modified area, it may be important to minimize the overlap. Because the alternate edges 312Aa, b, c may not traverse redundant areas of space (or the amount of traversed redundant space may be reduced), the alternate edges 312Aa, b, c can maximize the regions available to the robot without duplicating potentially blocked regions.

The alternate waypoint generator <NUM> may adjust the navigation route <NUM> to include at least one of the one or more alternate waypoints 310Aa, b that bypass the route edge(s) <NUM> blocked by the obstacle <NUM>. As discussed in more detail below, in some implementations, the alternate waypoint generator <NUM> generates the alternate waypoints 310Aa, b using a polyline buffer algorithm. The alternate waypoint generator <NUM> may apply one or more polyline buffer algorithms to a flattened two-dimensional map (e.g., the graph map <NUM>) to generate candidate alternate waypoints 310Aa, b. The alternate waypoint generator <NUM> may evaluate all or a portion of the candidate alternate waypoints 310Aa, b using one or more constraints and/or checks to verify the validity of the candidate alternate edge. For example, the alternate waypoint generator <NUM> can perform collision checking via signed distance fields, perform ground-height variation checks, determine a maximum recorded and/or nominal path length bypassed by the candidate alternate waypoint 310Aa, b, and/or check for compatibility with user configuration options (e.g., the user may disable alternate waypoint 310Aa, b generation in some scenarios). When the candidate alternate waypoint 310Aa, b passes all or a portion of the checks and/or constraints, the alternate waypoint generator <NUM> may confirm the alternate waypoint 310Aa, b for inclusion in the graph map <NUM> and/or the adjusted navigation route 212A. The alternate waypoint generator <NUM> may perform similar checks and evaluations for candidate alternate edges 312Aa, b, c along with appropriate additional checks (e.g., determining whether the alternate edge causes a maximum recorded or nominal path length to be exceeded).

As shown in <FIG>, the local navigation module <NUM> may receive the adjusted navigation route 212A and navigate the robot <NUM> accordingly. The adjusted navigation route 212A may include one or more adjusted boundary constraints 320A. An additional boundary constraint 320A centered on the alternate edge 312A may enable the robot <NUM> the freedom to traverse the alternate edges 312Aa, b, c. The alternate waypoint generator <NUM> may generate new boundary constraints <NUM> (or, alternatively, adjust current boundary constraints <NUM>) based on the generated alternate waypoints 310Aa, b and/or alternate edges 312Aa, b, c. The adjusted navigation route 212A includes the alternate waypoints 310Aa, 310Ab and the robot <NUM> navigates (e.g., first) to the alternate waypoint 310Aa via the edge 312Aa to the second alternate waypoint 310Ab via the edge 312Ab. The local navigation module <NUM> may navigate the robot <NUM> from a route waypoint 310a to an alternate waypoint 310Aa, b included in the adjusted navigation route 212A by traversing the robot <NUM> along the respective alternate edge 312Aa, b, c connecting the alternate waypoint 310Aa, b to the route waypoint 310a. In this example, the robot <NUM> may bypass the obstacle <NUM> utilizing the alternate waypoints 310Aa, b and return to the original navigation route <NUM> at the route waypoint 310b.

In some examples, the navigation system <NUM> determines that (e.g., while traversing an obstacle <NUM>) an alternate edge 312Aa, b, c connecting an alternate waypoint 310Aa, b and a route waypoint 310b or another alternate waypoint 310Aa, b, c is blocked (e.g., by the same obstacle <NUM> blocking the route waypoint <NUM> or a separate obstacle <NUM>). The navigation system <NUM> may readjust the adjusted navigation route 212A to include one or more additional alternate waypoints 310Aa, b. The readjusted navigation route 212A thus bypasses the blocked alternate edge 312Aa, b, c.

In some implementations, all or a portion of the route waypoints 310a, b, the alternate waypoints 310Aa, b, the route edge <NUM>, and/or the alternate edges 312Aa, b, c is associated with a respective cost. The navigation system <NUM>, when determining the navigation route <NUM> and the adjusted navigation route 212A, may obtain the cost associated with all or a portion of the waypoints 310a, b, 310Aa, b and/or edges <NUM>, 312Aa, b, c. The navigation system <NUM> may generate the adjusted navigation route 212A using the route edges <NUM> and alternate edges 312Aa, b, c (and or waypoints <NUM>, 310Aa, b) that have a lowest aggregate cost. In some embodiments, route edges <NUM> have a lower cost than alternate edges 312Aa, b, c to incentivize the robot <NUM> to return to the original navigation route <NUM> after bypassing the obstacle <NUM> (e.g., as soon as possible after bypassing the obstacle <NUM>).

Referring now to <FIG>, an exemplary graph map 222a includes a navigation route <NUM> populated with a series of route waypoints <NUM> connected by route edges <NUM>. In some examples, the graph map 222a is generated based on sensor data captured by a robot during a previous mission. For example, the robot may autonomously or semi-autonomously (e.g., at least partially manually controlled by a user computing device) traverse an environment while performing simultaneous localization and mapping (SLAM). The robot may periodically or aperiodically "drop" a route waypoint <NUM> as the robot traverses the environment. The robot may capture sensor data (e.g., using the sensor system) at all or a portion of the route waypoints <NUM> to generate the graph map 222a.

Referring now to <FIG>, an exemplary graph map 222b includes an adjusted navigation route 212A (e.g., an adjusted version of the navigation route as shown in <FIG>) populated with a series of route waypoints <NUM> connected by the same route edges <NUM>. The navigation route 212A is populated with alternate waypoints 310A (and the corresponding alternate edges 312A) at all applicable locations throughout the graph map 222b. In some implementations, the alternate waypoint generator generates an alternate waypoint 310A at every location on the graph map <NUM> that meets the criteria for alternate waypoint 310A generation. The criteria for generating an alternate waypoint 310A may include sufficient distance (e.g., the offset distance) between a respective route waypoint <NUM> and obstacles represented in the graph map 222b. Therefore, the alternate waypoint generator may generate alternate waypoints 310A based on the alternate waypoint generator determining (via the data from the graph map 222b) that there is sufficient room for the robot to traverse the alternate waypoint 310A without colliding with an obstacle indicated by the graph map 222b.

Referring now to <FIG>, in some examples, the alternate waypoint generator may generate alternate waypoints 310A within a threshold distance of the robot. An exemplary graph map 222c indicates a current position of the robot and a bounding box <NUM> that represents a valid area for the alternate waypoint generator to generate alternate waypoints 310A. By constraining the area that the alternate waypoint generator generates alternate waypoints 310A, the navigation system <NUM> may reduce resources (e.g., computational or temporal) necessary to generate the adjusted navigation route 212A. In these examples, the alternate waypoint generator may generate alternate waypoints 310A near a current location of the robot all or a portion of the times the robot's progress is impeded (e.g., the robot gets "stuck"). The threshold distance may vary based on direction from the robot. For example, the alternate waypoint generator may generate alternate waypoints 310A that are within a first threshold distance in front of the robot (e.g., ten meters, twenty meters, thirty meters, thirty-five meters, etc.) and within a second threshold distance behind the robot (e.g. five meters, ten meters, twenty meters, etc.).

Referring now to <FIG>, in some implementations, the alternate waypoint generator generates several candidate alternate waypoints. As shown in the schematic view 600a of <FIG>, the high-level navigation module may initially generate the graph map using inputs such as the desired path (e.g., a prior recorded path selected by the user computing device or otherwise representative of the desired mission for the robot), point clouds generated from the sensor data captured by the sensor system, and/or local grid maps. A schematic view 600b of <FIG> illustrates exemplary candidate alternate waypoints generated from a graph map. The alternate waypoint generator may generate candidate alternate waypoints from all or a portion of the route waypoints. Each candidate alternate waypoint may be offset from the route waypoint by the offset distance. A schematic view 600c of <FIG> provides an enhanced view of the route waypoints, route edges, the candidate alternate waypoints, and the corresponding alternate edges. The navigation system may perform any number of constraint checks and/or evaluations prior to confirming all or a portion of the candidate alternate waypoints.

Referring now to <FIG>, in some implementations, to generate the candidate alternate waypoints 310A and alternate edges 312A, the alternate waypoint generator determines chain transforms of one or more route waypoints <NUM> of the navigation route <NUM> (e.g., starting from the "oldest" route waypoint <NUM>) and converts to two-dimensional geometry. As illustrated in schematic views 700a-d, the alternate waypoint generator may determine a series of offset polygons (e.g., "buffers") out to a threshold distance (e.g., <NUM> meters) using one or more polyline-buffer algorithms. The alternate waypoint generator, in some examples, iteratively corresponds the outermost ("rim") of the alternate waypoints 310A to the nearest point on the polygon buffer and evaluates the straight line (the "spoke" or alternate edge 312A) formed by buffer points <NUM> and the candidate alternate waypoint 310A. The alternate waypoint generator, in some examples, will skip a buffer point <NUM> when the buffer point <NUM> crosses a route edge <NUM> or when the buffer point <NUM> is within a threshold distance of a route waypoint <NUM>. The alternate waypoint generator may also skip the buffer point <NUM> when the hub waypoint <NUM> crosses another spoke. The alternate waypoint generator computes new transforms for spokes (alternate edges 312A) and rim edges (alternate edges 312A between adjacent alternate waypoints 310A). In some examples, the alternate waypoint generator connects successive rim waypoints (alternate waypoints 310A) when the waypoints create a small loop. As seen in <FIG>, the alternate waypoint generator may determine whether the path between all or a portion of the alternate waypoints 310A of the candidate rim edge 312A (e.g., the shortest path as compared to other paths between the alternate waypoints 310A) satisfies a threshold.

The alternate waypoint generator may evaluate alternate edges 312A between alternate waypoints 310A to determine whether the alternate edges 312A satisfy one or more conditions. For example, the alternate edges 312A connecting adjacent alternate waypoints 310A may be required to be within a threshold distance of the nearest route waypoint <NUM>. The alternate edges 312A may be evaluated by generating the buffer points <NUM> between all or a portion of the alternate waypoints 310A and route waypoint <NUM> closest (e.g., spatially) to the alternate waypoint 310A on the original graph map <NUM>. The alternate edge 312A that the robot <NUM> is capable of navigating may be drawn between the alternate waypoint 310A and the route waypoint <NUM> along the corresponding buffer points <NUM> and this process may be iterated for all or a portion of the alternate waypoints 310A within the graph map <NUM> or within a threshold distance of the robot <NUM>. For all or a portion of the alternate edges 312A connecting an alternate waypoint 310A and the route waypoint <NUM>, the alternate waypoint generator may verify that all or a portion of the alternate edges 312A are free of obstacles and within the sensing threshold range of the robot <NUM>. When the alternate edge 312A connecting the alternate waypoint 310A and the route waypoint <NUM> is not free of obstacles or within the sensing threshold range, the alternate waypoint generator may shorten or relocate the alternate waypoint 310A to bring the alternate edge 312A into compliance. Once the alternate waypoints 310A are evaluated (and in some implementations adjusted according to the alternate edges 312A connecting alternate waypoints 310A and the route waypoints <NUM>), the alternate waypoint generator may evaluate the alternate edges 312A connecting adjacent alternate waypoints 310A to be free of obstacles and to be within a threshold distance of at least one route waypoint <NUM>. Therefore, the alternate edges 312A connecting adjacent alternate waypoints 310A may not be within a sensing range of a current location of the robot (at a route waypoint <NUM>) if the alternate edge 312A is within the threshold distance of another route waypoint <NUM>.

Thus, the navigation system provides additional flexibility in a way that can be tuned and verified offline while runtime behavior remains deterministic. The robot may have a limited number of paths to avoid an obstacle or blockage of the nominal navigation route <NUM> and the navigation system may identify all or a portion of the paths. Additionally, the robot may return to the nominal (original) path after traversing the obstacle. The alternate waypoint generator may generate the alternate waypoints 310A and the alternate edges 312A either offline (e.g., prior to the robot traversing the navigation route <NUM>) or online (e.g., after the robot has encountered the blocked route edge <NUM>). For example, the alternate waypoint generator generates the alternate waypoints 310A prior to encountering a blocked route edge <NUM> (or route waypoint <NUM>) and enables the alternate waypoints 310A after encountering the blocked route edge <NUM>. Alternatively, the robot may rely on a higher cost of the alternate waypoint 310A to maintain the original navigation route <NUM> whenever possible. In other examples, the alternate waypoint generator generates the alternate waypoints 310A after encountering the blocked route edge <NUM>. In some examples, the alternate waypoint generator may not generate alternate waypoint 310A in specified regions. For example, the user computing device may specify regions (e.g., via a controller) where alternate waypoints 310A are prohibited.

<FIG> is a flowchart of an exemplary arrangement of operations for a method <NUM> of providing alternate route finding for waypoint-based navigation maps. The computer-implemented method <NUM>, when executed by data processing hardware causes the data processing hardware to perform operations. At operation <NUM>, the method <NUM> includes obtaining a topological map that includes a series of route waypoints. The topological map may further include a series of route edges. All or a portion of the route edges in the series of route edges may connect a corresponding pair of adjacent route waypoints in the series of route waypoints. The series of route waypoints and the series of route edges may be representative of a navigation route for a robot to follow from a start location to a destination location.

The method <NUM>, at operation <NUM>, includes receiving image data of an environment of the robot from an image sensor. At operation <NUM>, determining, using the image data, that a route edge of the series of route edges is blocked by an obstacle. For example, the operation <NUM> may include determining that the route edge is blocked by the obstacle as the robot navigates along the navigation route from a first route waypoint in the series of route waypoints to a second route waypoint in the series of route waypoints. The route edge may connect the first route waypoint and the second route waypoint. The method <NUM>, at operation <NUM>, includes generating, using the image data and the topological map, an alternate waypoint and an alternate edge. The alternate waypoint may be offset from at least one route waypoint in the series of route waypoints by an offset distance. The alternate edge may connect the alternate waypoint to the first route waypoint. The operation <NUM> may include generating an alternate edge for all or a portion of a plurality of alternate waypoints. The method <NUM>, at operation <NUM>, includes adjusting the navigation route to include the alternate waypoint and the alternate edge. The alternate waypoint and the alternate edge may bypass the route edge blocked by the obstacle. In some embodiments, the method <NUM> includes navigating the robot from the first route waypoint to the alternate waypoint by traversing the robot along the alternate edge connecting the alternate waypoint to the first route waypoint.

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 computer-implemented method when executed by data processing hardware (<NUM>) of a robot (<NUM>) causes the data processing hardware (<NUM>) to perform operations comprising:
obtaining a topological map comprising a series of route waypoints (310a, 310b) representative of a navigation route (<NUM>) for the robot (<NUM>) to follow from a start location to a destination location, wherein navigation by the robot (<NUM>) using the navigation route (<NUM>) is constrained by a first boundary constraint (<NUM>) for the robot (<NUM>), wherein the first boundary constraint (<NUM>) for the robot (<NUM>) is based on the series of route waypoints (310a, 310b);
receiving image data (<NUM>) of an environment (<NUM>) of the robot (<NUM>) from an image sensor;
determining, using the image data (<NUM>) and the first boundary constraint (<NUM>) for the robot (<NUM>), that a route edge (<NUM>) that connects a first route waypoint (310a) of the series of route waypoints (310a, 310b) to a second route waypoint (310b) of the series of route waypoints (310a, 310b) is at least partially blocked by an obstacle (<NUM>);
generating, using the image data (<NUM>) and the topological map, an alternate waypoint (310Aa) and an alternate edge (312Aa) connecting the alternate waypoint (310Aa) to the first route waypoint (310a); and
adjusting the navigation route (<NUM>) to include the alternate waypoint (310Aa) and the alternate edge (312Aa), the alternate waypoint (310Aa) and the alternate edge (312Aa) bypassing the route edge (<NUM>) that is at least partially blocked by the obstacle (<NUM>), wherein navigation by the robot (<NUM>) using the adjusted navigation route (212A) is constrained by a second boundary constraint (320A) for the robot (<NUM>), wherein the second boundary constraint (320A) for the robot (<NUM>) is based on the alternate waypoint (310Aa, 310Ab).