Dynamic window approach using optimal reciprocal collision avoidance cost-critic

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 , wherein converting the preferred velocity to a holonomic velocity includes increasing the radius of the one or more robots by a maximum distance between a preferred trajectory and a straight-line trajectory.

23. The system of claim 13 , 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 =α*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 i −v pref )+penalty+α d*d ( P,v t )

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.

24. The method of claim 22 , 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 v t .

25. A system, comprising a plurality of robots and a supervisory system wherein one or more robots under the supervision of the supervisory system perform the method of claim 1 .