Advanced behavior engine

• US 8,571,745 B2
• Filed: 05/11/2011
• Issued: 10/29/2013
• Est. Priority Date: 04/10/2008
• Status: Active Grant

• Current Assignee: iRobot Corporation
• Original Assignee: iRobot Corporation
• Inventors: Pack, Robert Todd, Taka, Orjeta
• Primary Examiner(s): Tran, Khoi
• Assistant Examiner(s): Bendidi, Rachid
• Application Number:
  US13/105,872
• Publication Number:
  US 20120010772A1
• Time in Patent Office: 902 Days
• Field of Search: 701/23, 701/24, 701/25, 701/26, 701/27, 700/245, 700/246, 700/247, 700/248, 700/249, 700/250, 700/251, 700/252, 700/253, 700/254, 700/255, 318/587
• US Class Current: 701/27
• CPC Class Codes: B25J 9/1664 characterised by motion, path, trajectory planning
• CPC Class Codes: G05B 2219/40519 Motion, trajectory planning
• CPC Class Codes: G05D 1/0088 characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours using knowledge based models G06N5/00
• CPC Class Codes: G05D 1/0238 using obstacle or wall sensors G05D1/0246 and G05D1/0289 take precedence; lidar systems designed for anti-collision purposes G01S17/93
• CPC Class Codes: G05D 1/0251 extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision stereoscopic image analysis H04N13/00; depth recovery from images G06T7/593
• CPC Class Codes: G05D 1/027 comprising intertial navigation means, e.g. azimuth detector inertial navigation G01C21/16; inertial navigation combined with non-inertial navigation instruments G01C21/165
• CPC Class Codes: G05D 1/0272 comprising means for registering the travel distance, e.g. revolutions of wheels measuring distance traversed on the ground by vehicles, e.g. using odometers G01C22/00
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30
• CPC Class Codes: G06N 3/008 based on physical entities controlled by simulated intelligence so as to replicate intelligent life forms, e.g. based on robots replicating pets or humans in their appearance or behaviour

Abstract

A method of generating a command for a remote vehicle controller by taking in goals and constraints compiled from behavior inputs and action models, and controlling resources by producing low-level actuator commands that realize goals expressed by the behaviors. The method comprises: executing a command based on a kinodynamic fixed depth motion planning (KFDMP) algorithm to plan paths with longer time horizons and to use incremental feedback from evaluators to select a best feasible course of action; and feeding the selected best feasible course of action as servo commands to a drive system or a manipulation system of the remote vehicle controller. The selected best feasible course of action comprises goal-achieving actions within a fixed time horizon of several seconds from the current time each time a course of action is selected.

Claims

1. A method of generating a command for a remote vehicle controller of a remote vehicle by taking in goals and constraints compiled from behavior inputs, and controlling resources by producing low-level actuator commands that realize goals expressed by the behaviors, the method comprising:

executing a command by a processor based on a kinodynamic fixed depth motion planning (KFDMP) algorithm to plan paths with longer time horizons and to use incremental feedback from evaluators to select a best feasible course of action;

electronically providing the best feasible course of action as servo commands to a drive system or a manipulation system of the remote vehicle controller, and

commanding one or more actuators of the remote vehicle by the remote vehicle controller based on the servo commands;

wherein the best feasible course of action comprises goal-achieving actions within a fixed time horizon of several seconds from the current time each time a course of action is selected.

2. The method of claim 1, wherein the best feasible actions is dynamically feasible and kinematically feasible.

3. The method of claim 1, further comprising selecting a “

good enough”

best feasible action that incorporates as much future consequence information as possible given various constraints.

4. The method of claim 1, wherein the KFDMP algorithm abstracts away remote vehicle kinematics and actuators so that same waypoint behaviors work on a variety of remote vehicles.

5. The method of claim 1, wherein the KFDMP algorithm comprises a search tree made of nodes that encode a kinematic and dynamic state variables of the remote vehicle'"'"'s drive system.

6. The method of claim 1, further comprising constructing a kinodynamic tree by adding nodes that represent a fixed, small Δ

T from the current time, wherein each node is added by assuming a command (acceleration) change is applied during a given time step and is integrated (simulated) through a forward dynamics model and a forward kinematic model of the particular remote vehicle drive system.

7. The method of claim 6, wherein the kinematic model comprises a plug-in object that covers separate classes of vehicles (e.g., cars, tanks, UGVs, etc.) such that a single kinematic model covers a large number of remote vehicles within a class by parameters set in the kinematic model.

8. The method of claim 7, wherein a plug-in model of dynamics and kinematics allows a planning engine of the KFDMP algorithm to handle arbitrary holonomic and non-holonomic drive systems with arbitrary constraints.

9. The method of claim 5, wherein each node of the tree is annotated with a collection of tracking variables in addition to basic dynamic state information, the tracking variables including:

a command that generated the node;

a node cost comprising a weighted sum of evaluators applied to the node;

an expansion index and expansion counter, the expansion index equal to the number of times expansion has been done on a node to drive expansion selection and the expansion counter equal to the number of child nodes added to a node in total; and

/or

a depth counter.

10. The method of claim 9, wherein the evaluators comprise functions that map nodes to a cost function representing the goals of the remote vehicle as set by the behaviors, and each evaluator handles a different part of the cost and nodes are evaluated based on their cost, which can be a weighted sum of evaluator costs.

11. The method of claim 5, wherein searching for a best feasible action comprises building a tree of a given search depth and accepting a certain maximum branching factor.

12. The method of claim 11, further comprising:

maintaining a priority queue of nodes by node cost;

inserting a root node into a best node queue;

maintaining a best expandable node queue containing the best nodes that can still be expanded; and

inserting a first node in both queues,

wherein the queues speed up core search operations of finding the node with the best cost subject to certain criteria.

13. The method of claim 12, further comprising refining the search tree made of nodes, and maintaining a refinement set of nodes to avoid refining the same pairs of nodes over and over.

14. The method of claim 13, wherein the search for a best feasible action proceeds by an alternating sequence of expand and refine operations applied to the tree.

15. The method of claim 14, wherein the KFDMP algorithm proceeds, alternating expand and refine operations on the tree, exploring future feasible options as best it can until one of following termination conditions is used to extract the best available command:

a time limit for getting an answer is reached;

the best node queue becomes empty (the tree is all expanded out);

or

the node cost of the best node goes below a “

good enough”

threshold in which case the search sends the current best feasible action at the time the node cost of the best node goes below the “

good enough”

threshold.

16. The method of claim 15, wherein a best command is extracted by picking the best node from the best node queue and tracing the parent links backward until the root node is located.

17. A robotics system comprising:

a behavior tree comprising a tree of behaviors for controlling remote vehicle resources, the behavior tree generating behavior goals;

one or more action models that generate remote vehicle constraints;

multiple remote vehicle resources;

a control arbiter for each remote vehicle resource, the control arbiter controlling its associated remote vehicle resource;

a remote vehicle controller in communication with the control arbiters; and

one or more resource controllers in communication with the remote vehicle controller, the resource controllers receiving the behavior goals and the remote vehicle constraints and generating an overall command which is sent to the remote vehicle controller for execution on the remote vehicle resources.