System and method for autonomous mobile robot relocalization

Robots may be programmed to complete complex tasks in an environment. Robots use various mechanisms to navigate throughout the environment and maintain its pose (position or location) within the environment. Depending on the design, sophistication, and purpose of the robot, it may employ one or more sensors such as image capture devices (e.g., an RGB camera, an infrared camera, etc.), ranging devices (e.g., radar, lidar, sonar, etc.), proximity sensors, or the like to navigate within the environment.

In the following description, for purposes of explanation, numerous specific details are set forth in order to provide a thorough understanding of some example embodiments. It will be evident, however, to one skilled in the art that the present disclosure may be practiced without these specific details.

The systems and methods described herein may be used to relocate a robot or other autonomous agent (e.g., vehicle, machine, droid, etc.) that has lost its pose. Pose refers to the robot's location and/or orientation in an environment or position on a map. A robot may lose its pose due to many causes, such as a malfunctioning sensor, corrupted data, a hardware or software fault, an operating system error, or by being “kidnapped” by being moved by an external force (e.g., a person who picks up and moves the robot). Once the robot loses its ability to determine its current location in a map that it was using or in a known environment that it was mapping, the robot has to rely on current sensor readings and attempt to determine its location based on searching the map or previous sensor readings. If the robot finds a match, then the robot can relocalize itself. Relocalization is the process of determining its current location and orientation. Relocalization may be performed by the robot or by a robot controller (e.g., a system that remotely controls one or more robots).

Relocalization algorithms are important for robot navigation. The relocalization can be divided in two different categories depending on whether the robot is equipped with a laser scanner, a camera module, or both. If the robot is equipped with a laser scanner, then conventional approaches use a Monte Carlo Localization (MCL) algorithm for relocalization. While this technique is well studied and can solve the relocalization problem in most scenarios, the MCL algorithm is heavily compute intensive. As such, robots would require higher-performing compute hardware to perform this algorithm. The present systems and methods use an improved approach that is more efficient and conserves compute time, battery, memory, and other resources.

If the robot is equipped with only a camera module, then conventional approaches may use a bag of words (BoW) technique for relocalization. The BoW technique is less accurate than other approaches. The BoW relocalization can provide an estimate of the robot pose, however, an additional optimization technique is needed to improve accuracy.

In scenarios where both a camera module and a laser scanner are available, then conventional approaches use a vision approach to find the estimated pose and further optimize the pose by laser scans. However, there is a need for a compute-efficient technique to better utilize laser scans when performing pose estimation.

The present system and methods address the relocalization problem by using laser scans to provide an accurate pose estimation with improved compute time and memory. Using the systems and methods described herein, autonomous vehicles, autonomous mobile robots, and drones have a fast, reliable relocalization solution for a safe navigation. Further, the techniques explained in this document can be used for a vision-based approach by reducing the vision space search space, as described further below. Additional details are provided below.

is a diagram illustrating a general relocalization technique, according to an example. In preoperational state, an autonomous mobile robot (AMR) is configured for operation. Configuring the AMR may include activities such as installing or initializing sensors, operating systems, runtime libraries, executables, programmable hardware, engines, batteries, locomotion mechanisms, and the like. For instance, an AMR for cleaning may be configured with a map of the environment it is designed to clean and one or more paths to use when traversing through the environment.

In operational state, the AMR is operated in an environment according to its use. For example, the cleaning AMR may operate to empty trash bins of offices on the floors of an environment. The AMR may be operable to move from office to office, sense walls, doors, trash bins, and other objects in the environment while it traverses one or paths configured by preoperational state. The cleaning AMR may include sensors (e.g., lidar, radar, cameras, GPS, indoor positioning systems, microphones, etc.) to determine its location and orientation in the map and sense potential obstacles, stairs, or other objects in the environment. During the operational state, the AMR may perform various other techniques including Simultaneous Localization and Mapping (SLAM) operations, alternative mapping operations, building models of its environment, tracking its location in space for navigation and positional monitoring, recording images, video, or audio, and the like. Data collected may be stored in case the AMR has to relocate itself.

In off path state, the AMR has lost its position. There are two main reasons why an AMR may lose its position: malfunction or external forces. An AMR may lose its position due to a malfunction of one or more of its systems. For instance, a sensor array may malfunction, which results in the AMR traversing to an area in an environment that was off path. An AMR may also lose its position by external forces. For instance, a person may disable the AMR, pick it up and move it to another location, and then reenable the AMR. This is referred to as a kidnapping scenario. In either case, the AMR now has to relocate itself to be able to get back on path.

In relocalization state, the AMR uses data that it has previously sensed, data that is currently being sensed, or data obtained by the AMR (e.g., over a wireless connection with a cloud service). The AMR may use a relocalization technique to identify its pose (location and orientation) in an environment (e.g., on a map).

One conventional technique used for relocalization is Adaptive Monte Carlo Localization (AMCL). AMCL uses a particle filter and works by utilizing laser scanning. A particle is a location in a map (e.g., an x-y coordinate) that may be a possible pose (location and orientation) of the AMR. For relocalization, AMCL randomly generates particles throughout the entire map. As the AMR moves and gathers more information from the environment, the particle weights get updated, and the AMCL converges by improving the uncertainty of a particle that represents the most likely pose.

Although AMCL can provide accurate localization estimates and is robust to sensor noise, due to the nature of MCL and the number of particles, it is compute intensive and requires a large convergence time. Further, AMCL requires high amounts of memory because the state of each particle needs to be stored.

Another technique that was conventionally used for relocalization is referred to as a Strategy to Find a Missing Person (SFMP). SFMP uses MCL and limits the number of particles generated in proximity of where the AMR was last localized. SFMP constructs a moving distance map to denote the displacement from the last known location point to reduce the search space.

The disadvantage of SFMP is that the relocalization approach works better if the last known location of the AMR is known. Because of the reliance on the last known location in its algorithm, SFMP does not provide any advantage for an initial relocalization on power up/activation, or when a robot is kidnapped (e.g., picking up the robot and placing it in a new environment). Also, as the elapsed time increases, the efficiency of this method decreases because a larger environment is needed to conduct the search.

The relocalization technique described herein has advantages over both AMCL and SFMP. The search environment is reduced by constructing a path from a plurality of milestones, the path having segments linked by milestones on the path (i.e., waypoints) and ranking the milestones of interest based on a connectivity value. The search is then conducted iteratively over subsets of the ranked milestones. The milestones are selected, grouped, and ranked differently depending on the reason why the AMR lost its position. These implementations are discussed further below.

For each subset, particle clouds are generated around the milestones. A particle is a possible pose or position of the robot. The particle clouds may be generated using a random sampling technique, such as with a Gaussian distribution. The number of particles generated at each milestone may be the same, different, adapted (e.g., based on a function), or configurable. A Monte Carlo Localization (MCL) or another localization technique is used on the selected milestones with the particle cloud that was generated. The number of milestones or the number of particles may be increased to increase the search space until a solution is found.

is a flowchart illustrating a relocalization technique, according to an example. The relocalization techniquedescribed inis for the situation where the localization failure is due to malfunction. As discussed above, malfunctions of sensors, software, hardware, communications, or other components may cause a localization failure at the AMR.

At operation, in an offline phase, a grid map is generated. The grid map may be an occupancy grid map and be based on the operating environment of an AMR. Using sensor data from one or more AMRs in the operating environment, together with other information about the environment that is already known (e.g. data regarding the size and location of static objects in the environment), a shared environment model that represents the environment is generated. This shared environment model may be represented as a navigation grid (occupancy grid) having cells of any suitable size and/or shape, with each cell having specific properties with respect to the type of object contained (or not contained) in the cell, whether an object in the cell is static or moving, etc., which enables the environment model to accurately depict the state of the environment.

As an example, grid cells may be squares of predetermined sizes (e.g., 80 mm) based upon a desired granularity for a particular environment and accompanying application. The environment model may thus be dynamically updated by AMRs directly on a cell-by-cell basis as new sensor data is received from the AMRs. The updates to the shared environment model thus reflect any recent changes in the environment such as the position and orientation of AMRs and other obstacles that may change in a dynamic manner within the environment (e.g. people, forklifts, machinery, etc.). The shared environment model may additionally or alternatively be updated based upon data received from other sensors or devices within the environment, such as stationary cameras for example, which may enable a more accurate depiction of the positions of obstacles and AMRs without relying on AMR communications.

At operation, the grid map of the AMR's environment is transformed into Probabilistic Roadmap or a Deterministic Roadmap (PRM/DRM). The number of milestones needed to construct a PRM/DRM may be chosen or intelligently determined based on heuristics. In an example, Adaptive Sampling techniques based on Intelligent Sampling and Two-way search (ITS) are used to determine the number of milestones.

At operation, the AMR is configured using the PRM/DRM generated from operation. The AMR may be configured to use some or all of the PRM/DRM during its operation. Either a PRM or a DRM works for the operations described herein.

At operation, the AMR is brought online (online phase) and uses the roadmap to navigate the environment. This is considered normal operational mode where the AMR uses the roadmap to move from location to location depending on tasks and objectives. At some point, the AMR suffers a malfunction of sensors, software, hardware, communications, or other components causing a localization failure and AMR loses its pose.

At operation, the last known location of the AMR is determined and a top N closest milestones are identified from the map based on a Nearest Neighbor (NN) search from the last known location. The AMR is in an offline phase. The NN search is a technique to find the closest data points to a given query point in a high-dimensional space. The NN search uses a search radius around a milestone to determine if any other milestones are within the radius. The number of milestones returned may be capped to some number, N, as the maximum milestones for a given iteration of the NN search.

Various types of NN search may be used. A non-limiting list includes using Linear Search, k-d Tree Search, Vector Approximation Files, Compression/Clustering-based search, Quadtree and Octree Search, Orchard's Algorithm and Annulus Algorithm, Principal Component Partitioning (PCP), R-Tree Search, dynamic search, vp tree search, bk tree search, and Locality sensitive hashing search.

The milestones selections are prioritized based on a path or intended trajectory. The path includes a source milestone, a goal milestone, and zero or more intermediate milestones between the source and goal milestones. Milestones that are part of the path may also be referred to as waypoints. In an example, each of the milestones in the path are included in the top N closest milestones along with milestones that are identified with the NN search.

At operation, particle clouds are generated around the top N selected milestones based on a Gaussian distribution. At operation, a localization technique (e.g., Monte Carlo Localization) is used on these N selected milestones.

If the solution is not found, then the techniquereturns to operationto select a next set of N ranked milestones from the PRM/DRM roadmap. The search is expanded by selecting the next N milestones found by increasing the radius used in the NN search. Additionally, the number of the particles are changed adaptively based on Equation 1, until a solution is found.

where, Δt is time elapsed since localization failed, v is robot speed, pis minimum number of particles, and pis the maximum number of particles allowed.

If a solution is found, then the localization technique is successful and a pose is determined (operation), the AMR is able to return to normal operation and be brough back online (operation).

is a flowchart illustrating a relocalization technique, according to an example. The relocalization techniquedescribed inis for the situation where the localization failure is due to external forces. As discussed above, external forces may include situations where a person picks up and moves a robot, when a robot is moved by an animal, another robot, or by an act of nature (e.g., earthquake), or similar situations.

At operation, in an offline phase, a grid map is generated. The grid map may be an occupancy grid map and be based on the operating environment of an AMR, as discussed above in operation.

At operation, a grid map is converted into a Probabilistic or Deterministic Roadmap (PRM/DRM). A grid map may be converted to either a Probabilistic Roadmap (PRM) or a Deterministic Roadmap (DRM). In a PRM the milestones are generated randomly and in a DRM the milestones are generated deterministically. Either a PRM or a DRM works for the operations described herein.

The number of milestones needed to construct a PRM/DRM may be chosen or intelligently determined based on heuristics. In an example, Adaptive Sampling techniques based on Intelligent Sampling and Two-way search (ITS) are used to determine the number of milestones.

At operation, the AMR is configured using the PRM/DRM generated from operation. The AMR may be configured to use some or all of the PRM/DRM during its operation. Either a PRM or a DRM works for the operations described herein.

At operation, the AMR is brought online (online phase) and uses the roadmap to navigate the environment. This is considered normal operational mode where the AMR uses the roadmap to move from location to location depending on tasks and objectives. Δt some point, the AMR suffers a malfunction of sensors, software, hardware, communications, or other components causing a localization failure and AMR loses its pose.

At operation, the milestones are ranked based on the number of the connectivity value. The connectivity value refers to the number of edges connecting to the milestone in the PRM/DRM. In addition to the number of connections to a milestone (connectivity), milestones may also be ranked based on historical data. For instance, milestones that are traversed more often or used more often in path planning are weighted higher than other milestones.

An example formula for determining a ranking value is: Ranking Value (R)=Connectivity Value (C)+Historical Visits (V), where C is the number of connections to a given milestone, and V is the number of visits the robot has made to the given milestone in some period. The period may be configurable but may be in terms of hours, days, weeks, etc.

For example, if a milestone is connected to three adjacent milestones, then this milestone has a rank of three. As another example, the connectivity may be calculated based on a different linear formula, polynomial formula, or other formula.

As another example, historical data may be used in the ranking formula. Milestones are ranked based on the number of times the robot has visited a milestone or an area where the milestone is located. One example is to keep track of how many times a robot has reached each milestone historically. As another example, in a warehouse with two different rooms (roomand room), and based on historical data over the last two weeks, it is determined that the robot has been in room, five times and it has been in room, twenty times. Thus, any milestones in roomare ranked twenty and any milestones in roomare ranked five.

At operation, the highest T ranked milestones are selected from the PRM/DRM map. At operation, particle clouds are generated around the T selected milestones base on a Gaussian distribution. At operation, a localization technique (e.g., Monte Carlo Localization) is used on these T selected milestones.

If the solution is not found, then the techniquereturns to operationto select a next set of T ranked milestones from the PRM/DRM map. The number of the particles to generate around each of the milestones is adaptively changed based on Equation 2.

where, S is the number of particles needed to draw for MCL, T is the number of selected top rank milestones, N is the number of nearest neighbor milestones (connectivity number) for each selected milestone, A is the desired step size for rotation in radian, pis the maximum number of particles allowed. The last argument controls how many particles are allowed per radian around a given milestone. In other words, the distribution of particles around a milestone per angle.

If a solution is found, then the localization technique is successful and a pose is determined (operation), the AMR is able to return to normal operation and be brough back online (operation).

is a diagram illustrating a roadmapof an environment for a use case. The roadmapmay be a PRM or DRM. The roadmapincludes occupied spacesA-F and unoccupied space. A number of milestonesA-N are distributed throughout the unoccupied space. A pathis made up of two or more milestonesA-N. The pathincludes milestonesB,D,F,H,K, andL. A robotnavigates from a source milestoneB to a goal milestoneL.

In a first scenario, the robottemporarily loses its sensing ability while navigating from milestoneD to milestoneF. Techniquemay be used in this situation. The last known position is identified as milestoneD. A top N closest milestones are identified using a Nearest Neighbor search. For instance, milestonesA,B,E andG may be identified with the NN search. Particle clouds are generated around these milestones and the MCL is used. If no solution is found, then the search is expanded by adjusting the NN search and the number of particles are increased using Equation 1.

If a second scenario occurred where the robotwas kidnapped to the position illustrated in, the techniquemay be used for relocalization. MilestonesA-N are ranked according to their connectivity values and historical usage data. For example, in the example illustrated in FIG., milestoneA may be ranked higher than milestoneE because milestoneA has a connectivity value of 5 and milestoneE has a connectivity value of 4. However, if milestoneE is used more often in path planning or has been traversed by the robot more often in a given period, then milestoneE may be ranked higher than milestoneA. The ranked milestones are considered in batches based on their rank (e.g., if the batch size is 10, then the first 10 are considered together, then the next 10 in the ranked list, etc.). Particle clouds are generated around the milestones being considered and the MCL is used. If no solution is found, then the next batch of ranked milestones are searched and the number of particles are changed using Equation 2.

is a flowchart illustrating a techniquefor relocalization, according to an example. The techniquemay be performed by a device including processing circuitry and memory. In an example, the processing circuitry and memory are part of a command device in communication with each of a set of robots. In another example, the processing circuitry is integrated with or connected to the robot. The memory may include instructions, to be executed by the processing circuitry. The instructions may include operations to control actions of the set of robots to relocate themselves or achieve a task without use of the command device.

The techniqueincludes an operationto determine a cause of a loss of pose. Loss of pose may be due to a malfunction of one or more components of the robot or because of a different cause, such as by movement due to an external force (e.g., a person picking up and placing the robot in a new location).

The techniqueincludes an operationto use a nearest neighbor process to select a set of milestones from a roadmap when the cause of the loss of pose is due to a malfunction, or use a ranking process to select the set of milestones from the roadmap when the cause of the loss of pose is not due to the malfunction, the roadmap including a plurality of milestones. In an example, the roadmap is a probabilistic roadmap. In another example, the roadmap is a deterministic roadmap.