Dynamic window approach using optimal reciprocal collision avoidance cost-critic

Patent metadata

Field	Value
Publication number	US-10429847-B2
Application number	US-201715712256-A
Country	US
Kind code	B2
Filing date	Sep 22, 2017
Priority date	Sep 22, 2017
Publication date	Oct 1, 2019
Grant date	Oct 1, 2019

Abstract

A method and system for navigation of a robot along a goal path and avoiding obstacles. The method includes receiving goal pose for one or more robots and determining a goal path for a first robot while avoiding moving and fixed obstacles of a received obstacle map. A first objective function is evaluated to select a preferred velocity from a generated set of candidate velocities, the selecting based on one or more weighted cost functions. A set of velocity obstacles created based on the poses of the one or more robots and the preferred velocity is used in evaluating a second objective function to determine the motion of the robot in the next time cycle. Creating the set of velocity objects includes converting the preferred velocity from a non-holonomic to a holonomic velocity.

First claim

We claim:

1. A method for navigation of a robot along a goal path and avoiding obstacles, comprising:
receiving a goal pose for a first robot;
determining a goal path for the first robot;
receiving an obstacle map;
receiving the pose of the first robot;
receiving the pose of one or more other robots;
generating a set of candidate velocities for the first robot;
evaluating, using a first objective function, the first set of candidate velocities;
selecting, based on the first objective function, a first preferred velocity of the first robot;
creating a set of velocity obstacles based on the pose(s) of the one or more other robots and the first preferred velocity of the first robot;
evaluating, using a second objective function, the set of candidate velocities;
selecting, based on the second objective function, a second preferred velocity for the first robot; and
moving the first robot based on the second preferred velocity.

2. The method of claim 1 , wherein the goal path comprises a path from a current pose of the first robot to the goal pose of the first robot.

3. The method of claim 1 , wherein goal pose of the robot is the pose of a fiducial associated product bin in an order fulfillment warehouse application.

4. The method of claim 1 , wherein the pose of the first robot is determined by one or more of many-to-many multiresolution scan matching (M3RSM), adaptive monte carlo localization (AMCL), geo-positioning satellite (GPS), fiducial information, and odometry-based on robot sensors.

5. The method of claim 1 , wherein generating the set of candidate velocities for the first robot assumes a candidate velocity over one or more time steps applying motion, obstacle, and inertial constraints to generate only candidate velocities having admissible trajectories.

6. The method of claim 1 , wherein the first objective function is comprised of one or more cost functions of the form:
G ( v ,ω)=α*heading( v ,ω)+β*dist( v ,ω)+γ*velocity( v ,ω),

where G(v,ω) is the objective function, α, β, γ are weights; heading(v,ω) is a measure of progress along the goal path; dist(v,ω) is the distance to the nearest obstacle (its “clearance”); and velocity(v,ω) is the forward velocity of the robot for a given candidate velocity (v,ω).

7. The method of claim 5 , wherein the first objective function further includes one or more of:
a path cost function which scores how much the candidate velocity would radiate from the goal path;
an obstacle cost function scoring proximity to obstacles; and
an oscillation cost function assigning higher costs to changes in rotational velocity from a previous preferred velocity.

8. The method of claim 5 , wherein the one or more cost functions invalidate a candidate velocity by assigning a highest cost score to the candidate velocity.

9. The method of claim 1 , wherein creating the set of velocity objects includes converting the preferred velocity from a non-holonomic to a holonomic velocity.

10. The method of claim 8 , wherein converting the preferred velocity to a holonomic velocity includes increasing the radius of the one or more other robots by a maximum distance between a preferred trajectory and a straight-line trajectory.

11. The method of claim 1 , wherein the second objective function is comprised of one or more cost functions of the form:
ORCA/DWA= C DWA +α ORCA *C ORCA

where C DWA is defined as:
C DWA =a *heading( v ,ω)+β*dist( v ,ω)+γ*velocity( v ,ω),

where α, β, γ are weights; heading(v,ω) is a measure of progress along the goal path; dist(v,ω) is the distance to the nearest obstacle; and velocity(v,ω) is the forward velocity of the robot for a given candidate velocity (v,ω); and
C ORCA is defined as:
C ORCA =α v ( v t −v pref )+penalty+α d *d ( P,v r )

where α d and α v are weights; v t is a candidate velocity being evaluated; v pref is the preferred velocity; P is the polygon formed by the union of VOs; d(P, v t ) is a measure of how much a candidate velocity violates the VOs; and penalty is a penalty cost imposed when a candidate velocity v t violates a VO.

12. The method of claim 10 , wherein cost function d (P, v t ) is a function of the minimum distance from the perimeter of polygon P to a point defined by the trajectory t reached by candidate velocity yr.

13. A robot system for navigation of a robot along a goal path and avoiding obstacles, comprising:
a transceiver;
a data storage device;
a data processor configured to retrieve instructions stored on the data storage device for execution by the data processor to:
receive a goal pose for a first robot;
determine a goal path for the first robot;
receive an obstacle map;
receive the pose of the first robot;
receive the pose of one or more other robots;
generate a set of candidate velocities for the first robot;
evaluate, using a first objective function, the first set of candidate velocities;
select, based on the first objective function, a first preferred velocity of the first robot;
create a set of velocity obstacles based on the pose(s) of the one or more other robots and the first preferred velocity of the first robot;
evaluate, using a second objective function, the set of candidate velocities;
select, based on the second objective function, a second preferred velocity for the first robot; and
move the first robot based on the second preferred velocity.

14. The system of claim 13 , wherein the goal path comprises a path from a current pose of the first robot to the goal pose of the first robot.

15. The system of claim 13 , wherein goal pose of the robot is the pose of a fiducial associated product bin in an order fulfillment warehouse application.

16. The system of claim 13 , wherein the pose of the first robot is determined by one or more of many-to-many multiresolution scan matching (M3RSM), adaptive monte carlo localization (AMCL), geo-positioning satellite (GPS), fiducial information, and odometry-based on robot sensors.

17. The system of claim 13 , wherein generating the set of candidate velocities for the first robot assumes a candidate velocity over one or more time steps applying motion, obstacle, and inertial constraints to generate only candidate velocities having admissible trajectories.

18. The system of claim 13 , wherein the first objective function is comprised of one or more cost functions of the form:
G ( v ,ω)=α*heading( v ,ω)+β*dist( v ,ω)+γ*velocity( v ,ω),

where G(v,ω) is the objective function, α, β, γ are weights; heading(v,ω) is a measure of progress along the goal path; dist(v,ω) is the distance to the nearest obstacle (its “clearance”); and velocity(v,ω) is the forward velocity of the robot for a given candidate velocity (v,ω).

19. The system of claim 17 , wherein the first objective function further includes one or more of:
a path cost function which scores how much the candidate velocity would radiate from the goal path;
an obstacle cost function scoring proximity to obstacles; and
an oscillation cost function assigning higher costs to changes in rotational velocity from a previous preferred velocity.

20. The system of claim 17 , wherein the one or more cost functions invalidate a candidate velocity by assigning a highest cost score to the candidate velocity.

21. The system of claim 13 , wherein creating the set of velocity objects includes converting the preferred velocity from a non-holonomic to a holonomic velocity.

22. The system of claim 20

Assignees

• Locus Robotics Corp

Inventors

• Moore Thomas
• Powers Bradley

Classifications

• G05D1/65 Primary

  Following a desired speed profile · CPC title

• G08G1/164

  Centralised systems, e.g. external to vehicles · CPC title

• G06Q10/047

  Optimisation of routes or paths, e.g. travelling salesman problem · CPC title

• G08G1/166

  for active traffic, e.g. moving vehicles, pedestrians, bikes · CPC title

• G01C21/206 Primary

  specially adapted for indoor navigation · CPC title