Multi-resolution scan matching with exclusion zones

• US 10,386,851 B2
• Filed: 09/22/2017
• Issued: 08/20/2019
• Est. Priority Date: 09/22/2017
• Status: Active Grant

• Current Assignee: Locus Robotics Corporation
• Original Assignee: Locus Robotics Corporation
• Inventors: Moore, Thomas, Powers, Bradley
• Primary Examiner(s): Sweeney, Brian P
• Application Number:
  US15/712,222
• Publication Number:
  US 20190094876A1
• Time in Patent Office: 697 Days
• Field of Search: 701 23
• CPC Class Codes: B25J 9/1666 Avoiding collision or forbidden zones
• CPC Class Codes: B65G 1/0492 with cars adapted to travel in storage aisles
• CPC Class Codes: B65G 1/1373 for fulfilling orders in warehouses
• CPC Class Codes: G01C 21/206 specially adapted for indoor navigation
• CPC Class Codes: G05D 1/024 in combination with a laser lasers per se H01S
• CPC Class Codes: G05D 1/0248 in combination with a laser lasers per se H01S
• CPC Class Codes: G05D 1/0257 using a radar radar systems designed for anti-collision purposes between land vehicles or between land vehicle and fixed obstacles G01S13/931
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30

Abstract

A method for navigating a robot from a current pose to a goal pose. A map represents obstacles and free space within an area for robot navigation. A matching map pyramid and an exclusion map pyramid are constructed based on the map and decimations from a highest resolution to successively lower resolutions of the map pyramids. Current pose for navigating from a current location to a goal pose along a goal path includes determining a search area and creating a search heap. Scoring of search tasks on the search heap determines a best candidate pose at a highest resolution matching map, or expands the search heap with search tasks at the next higher resolution matching map. Expanding the search heap at the next higher resolution matching map avoids search tasks that would localize the robot in an exclusion zone.

Claims

1. A method for navigating a robot from a current pose to a goal pose, comprising:

receiving, by a transceiver of the robot, a map representing obstacles and free space within an area for robot navigation;

constructing, by a data processor of the robot, based on the received map, a matching map pyramid comprising a highest resolution matching map and a set of decimated matching maps at successively lower resolutions;

constructing, by the data processor of the robot, based on the received map, an exclusion map pyramid comprising a highest resolution exclusion map and a set of decimated exclusion maps at successively lower resolutions, each exclusion map including at least one exclusion zone associated with an obstacle of the received map;

receiving a goal pose for navigating a robot from its present location to the goal pose along a goal path;

scanning, by a scanner of the robot, to generate a scan indicating free space and obstacles in proximity to the robot at its present location;

finding the current pose of the robot; and

moving the robot to a next pose on the goal path in the direction of the goal pose;

wherein finding the current pose of the robot comprises;

determining, by the data processor, a search area within the received map;

creating a search heap comprising search tasks representing candidate poses at one or more resolutions of the matching map pyramid; and

comparing the search tasks on the search heap to the scan and scoring the comparison of each search task to determine a best candidate pose,

wherein if the best candidate pose is at the highest resolution matching map, the pose is selected as the current pose;

wherein if the best candidate pose is not at the highest resolution matching map the search heap is expanded based on the best candidate pose at a next highest resolution matching map; and

wherein the search heap is not expanded with search tasks with candidate poses that would localize the robot in an exclusion zone of an exclusion map at the next highest resolution exclusion map.

2. The method of claim 1, wherein the received map comprises a simultaneous localization and mapping (SLAM) map.

3. The method of claim 1, wherein creating the search heap and scoring the search tasks is performed by many-to-many multi-resolution scan matching (M3RSM).

4. The method of claim 1, wherein constructing the highest resolution matching map includes increasing the size of obstacles by a quadratic decay convolution filter.

5. The method of claim 1, wherein constructing the highest resolution exclusion map includes increasing the size of obstacles based on the radius of the robot.

6. The method of claim 1, wherein the decimating to construct each successively lower resolution of the matching map pyramid overestimates the scan match scoring in the next higher resolution matching map.

7. The method of claim 1, wherein the decimating to construct each successively lower resolution of the exclusion map pyramid underestimates the exclusions zones in the next higher resolution exclusion map.

8. The method of claim 7, wherein the underestimating of the exclusion zones at a lower resolution exclusion map and the not expanding search tasks localized in exclusions zones also prevents creating search tasks in exclusion zones at higher resolutions of the exclusion pyramid.

9. The method of claim 8, wherein the obstacles associated with the exclusion zones include one or more of walls, shelving, beams, bins, or charging stations.

10. A robot system, comprising

a laser-radar scanner;

a transceiver;

a data processor; and

a data storage device having instructions stored thereon for execution by the data processor to;

receive a map representing obstacles and free space within an area for robot navigation;

construct, based on the received map, a matching map pyramid comprising a highest resolution matching map and a set of decimated matching maps at successively lower resolutions;

construct, based on the received map, an exclusion map pyramid comprising a highest resolution exclusion map and a set of decimated exclusion maps at successively lower resolutions, each exclusion map including at least one exclusion zone associated with an obstacle of the received map;

receive a goal pose for navigating a robot from its present location to the goal pose along a goal path;

scan, by the laser-radar scanner, to generate a scan indicating free space and obstacles in proximity to the robot at its present location;

find the current pose of the robot; and

move the robot to a next pose on the goal path in the direction of the goal pose;

wherein finding the current pose of the robot comprises;

determine a search area within the received map;

create a search heap comprising search tasks representing candidate poses at one or more resolutions of the matching map pyramid;

compare the search tasks on the search heap to the scan and score the comparison of each search task to determine a best candidate pose,

wherein if the best candidate pose is at the highest resolution matching map, the pose is selected as the current pose;

wherein if the best candidate pose is not at the highest resolution matching map the search heap is expanded based on the best candidate pose at a next highest resolution matching map; and

wherein the search heap is not expanded with search tasks with candidate poses that would localize the robot in an exclusion zone of an exclusion map at the next highest resolution exclusion map.

11. The system of claim 10, wherein the received map comprises a simultaneous localization and mapping (SLAM) map.

12. The system of claim 10, wherein creating the search heap and scoring the search tasks is performed by many-to-many multi-resolution scan matching (M3RSM).

13. The system of claim 10, wherein constructing the highest resolution matching map includes increasing the size of obstacles by a quadratic decay convolution filter.

14. The system of claim 10, wherein constructing the highest resolution exclusion map includes increasing the size of obstacles based on the radius of the robot.

15. The system of claim 10, wherein the decimating to construct each successively lower resolution of the matching map pyramid overestimates the scan match scoring in the next higher resolution matching map.

16. The system of claim 10, wherein the decimating to construct each successively lower resolution of the exclusion map pyramid underestimates the exclusions zones in the next higher resolution exclusion map.

17. The system of claim 16, wherein the underestimating of the exclusion zones at a lower resolution exclusion map and the not expanding search tasks localized in exclusions zones also prevents creating search tasks in exclusion zones at higher resolutions of the exclusion pyramid.

18. The system of claim 17, wherein the obstacles associated with the exclusion zones include one or more of walls, shelving, beams, bins, or charging stations.

19. A method for determining a current pose of a robot comprising:

receiving, by a transceiver of the robot, a map representing obstacles and free space within an area for robot navigation;

constructing, by a data processor of the robot, based on the received map, a matching map pyramid comprising a highest resolution matching map and a set of decimated matching maps at successively lower resolutions;

constructing, by the data processor of the robot, based on the received map, an exclusion map pyramid comprising a highest resolution exclusion map and a set of decimated exclusion maps at successively lower resolutions, each exclusion map including at least one exclusion zone associated with an obstacle of the received map;

scanning, by a scanner of the robot, to generate a scan indicating free space and obstacles in proximity to the robot at its present location;

determining, by the data processor, a search area within the received map;

creating a search heap comprising search tasks representing candidate poses at one or more resolutions of the matching map pyramid, wherein creation of the search heap includes eliminating search tasks associated with candidate poses located in the at least one exclusion zone from the search heap; and

comparing the search tasks on the search heap to the scan and scoring the comparison of each search task to determine the current pose.