Advanced behavior engine

• US 9,327,403 B2
• Filed: 09/25/2013
• Issued: 05/03/2016
• Est. Priority Date: 04/02/2008
• Status: Active Grant

• Current Assignee: iRobot Corporation
• Original Assignee: iRobot Corporation
• Inventors: Pack, Robert Todd, Taka, Orjeta
• Primary Examiner(s): Holloway, Jason
• Assistant Examiner(s): Bendidi, Rachid
• Application Number:
  US14/036,451
• Publication Number:
  US 20140081506A1
• Time in Patent Office: 951 Days
• Field of Search: 700245-264, 701/23, 701/24, 701/25, 701/26, 701/27
• US Class Current: 1/1
• 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 commanding a remote vehicle includes executing a command on a controller of the remote vehicle based on a kinodynamic fixed depth motion planning algorithm to use incremental feedback from evaluators to select a best feasible action. The method also includes determining servo commands corresponding to the best feasible action for one or more actuators of a drive system or a manipulation system of the remote vehicle and commanding the one or more actuators of the remote vehicle based on the servo commands. The best feasible action includes actions within a fixed time horizon of several seconds from a current time each time a feasible action is selected.

Claims

1. A method of commanding a remote vehicle, the method comprising:

constructing a kinodynamic tree by adding nodes that represent a fixed difference in time from a current time, wherein each node is added by assuming a command change is applied during a given time step and is integrated through a forward dynamics model and a forward kinematic model of the remote vehicle;

executing a command on a controller of the remote vehicle based on a kinodynamic fixed depth motion planning algorithm to use incremental feedback from evaluators to select a best feasible action, the best feasible action comprising actions within a fixed time horizon of several seconds from the current time each time a feasible action is selected, wherein the kinodynamic fixed depth motion planning algorithm comprises the kinodynamic tree, the kinodynamic tree having nodes that encode kinematic and dynamic state variables of the resources of the remote vehicle;

determining, on the controller, resource commands corresponding to the best feasible action for commanding resources of the remote vehicle; and

commanding, using the controller, the resources of the remote vehicle based on the resource commands.

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

3. The method of claim 1, further comprising searching the kinodynamic tree for a node that corresponds to the best feasible action.

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

5. The method of claim 4, 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.

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

7. The method of claim 6, wherein the kinodynamic fixed depth motion planning algorithm searches for future feasible actions until one of following termination conditions is used to extract the best feasible action:

a time limit is reached;

the best node queue becomes empty when the kinodynamic tree is all expanded out;

or

the node cost of the best node goes below a threshold cost, in which case the search returns the current best feasible action at the time the node cost of the best node goes below the threshold cost.

8. The method of claim 7, wherein the best feasible action is extracted by picking the best node from the best node queue and tracing the parent links backward until the root node is located.

9. The method of claim 1, further comprising setting parameters of the forward kinematic model to cover the remote vehicle, the forward kinematic model covering separate classes of vehicles and multiple vehicles within a class of vehicles based on the parameters.

10. The method of claim 9, wherein a plug-in model of dynamics and kinematics allows a planning engine of the kinodynamic fixed depth motion planning algorithm to handle resources comprising holonomic or non-holonomic drive systems.

11. The method of claim 1, wherein each node of the kinodynamic tree is annotated with a collection of tracking variables in addition to dynamic state information, the tracking variables comprising:

a command that generated the node;

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

an expansion index and an 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.

12. The method of claim 11, wherein the evaluators comprise functions that map nodes to a cost function representing goals of the remote vehicle, each evaluator handling a different part of the cost, wherein nodes are evaluated based on their cost.

13. A robotics system comprising:

system resources;

a control arbiter for each system resource, each control arbiter controlling its associated system resource;

a system controller in communication with the control arbiters, the system controller;

constructing a kinodynamic tree by adding nodes that represent a fixed difference in time from a current time, wherein each node is added by assuming a command change is applied during a given time step and is integrated through a forward dynamics model and a forward kinematic model of the remote vehicle;

executing a command based on a kinodynamic fixed depth motion planning algorithm to use incremental feedback from evaluators to select a best feasible action, the best feasible action comprising actions within a fixed time horizon of several seconds from the current time each time a feasible action is selected, wherein the kinodynamic fixed depth motion planning algorithm comprises the kinodynamic tree, the kinodynamic tree having nodes that encode kinematic and dynamic state variables of the resources of the remote vehicle;

determining resource commands corresponding to the best feasible action for the system resources; and

communicating the resource commands to at least one control arbiter for commanding affected system resources.

14. The robotics system of claim 13, wherein the system controller:

generates a behavior tree comprising behaviors for controlling the system resources, the behavior tree generating behavior goals;

executes one or more action models that generate system constraints;

generates an overall command based on the behavior goals and system constraints; and

generates the resource commands to effect the overall command.

15. The robotics system of claim 13, wherein each node of the kinodynamic tree is annotated with a collection of tracking variables in addition to dynamic state information, the tracking variables comprising:

a command that generated the node;

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

an expansion index and an 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.

16. The method of claim 15, wherein the evaluators comprise functions that map nodes to a cost function representing goals of the robotics system, each evaluator handling a different part of the cost, wherein nodes are evaluated based on their cost.