Advanced behavior engine

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.