Systems and methods for incorporating latent states into robotic planning

1. A system for incorporating latent states into robotic planning, the system comprising:

a processor, the processor having:

a team module, implemented via the processor, configured to:

identify an agent team including at least one robotic agent and at least one human agent, and

receive sensor data associated with relative physical parameters between the at least one robotic agent and the at least one human agent;

a curiosity module, implemented via the processor, configured to:

model the latent states of the at least one human agent as a behavior model by calculating an expected total reward of the at least one human agent for executing an action, wherein the latent states describe cognition of the at least one human agent,

calculate a first belief state based on the relative physical parameters and the behavior model, and

predict future probabilities of future observations at a second time based on the first belief state; and

a planning module, implemented via the processor, configured to:

update a motion plan based on the future probabilities, and

cause the at least one robotic agent to execute the updated motion plan.

2. The system of claim 1 , wherein the team module is further configured to receive updated sensor data at the second time and determine updated relative physical parameters based on the updated sensor data, and

wherein the curiosity module is further configured to calculate a second belief state based on the updated relative parameters and the behavior model, and predict updated probabilities based on the second belief state.

3. The system of claim 2 , wherein the behavior model is refined based the updated sensor data.

4. The system of claim 2 , wherein the curiosity module is further configured to determine a probe maneuver, and wherein the updated sensor data is received in response to the probe maneuver being executed.

5. The system of claim 4 , wherein determining the probe maneuver is based on a curiosity reward proportional to an amount of information expected to be gained about the human agent.

6. The system of claim 1 , wherein the expected total reward is calculated using a quantal level reasoning model that includes a rationality coefficient that controls the degree to which the at least one human agent conforms to optimal behaviors.

7. The system of claim 1 , wherein the at least one robotic agent is a host vehicle and the at least one human agent is a proximate vehicle driven by a human, and wherein the motion plan is based on a planned maneuver of the host vehicle.

8. A method for incorporating latent states into robotic planning, the method comprising:

identifying an agent team including at least one robotic agent and at least one human agent;

receiving sensor data associated with relative physical parameters between the at least one robotic agent and the at least one human agent;

modeling the latent states of the at least one human agent as a behavior model by calculating an expected total reward of the at least one human agent for executing an action, wherein the latent states describe cognition of the at least one human agent;

calculating a first belief state based on the relative physical parameters and the behavior model;

predicting future probabilities of future observations at a second time based on the first belief state;

updating a motion plan based on the future probabilities; and

causing the at least one robotic agent to execute the updated motion plan.

9. The method of claim 8 , further comprising:

receiving updated sensor data at the second time;

determining updated relative physical parameters based on the updated sensor data;

calculating a second belief state based on the updated relative parameters and the behavior model, wherein the future probabilities are based on the second belief state;

predicting updated probabilities of future observations based on the second belief state; and

updating the motion plan based on the updated probabilities.

10. The method of claim 9 , wherein the behavior model is refined based the updated sensor data.

11. The method of claim 9 , further comprising determining a probe maneuver, wherein the updated sensor data is received in response to the probe maneuver being executed.

12. The method of claim 11 , wherein determining the probe maneuver is based on a curiosity reward proportional to an amount of information expected to be gained about the at least one human agent.

13. The method of claim 8 , wherein the expected total reward is calculated using a quantal level reasoning model that includes a rationality coefficient that controls the degree to which the at least one human agent conforms to optimal behaviors.

14. A non-transitory computer readable storage medium storing instructions that when executed by a computer having a processor to perform a method for incorporating latent states into robotic planning, the method comprising:

identifying an agent team including at least one robotic agent and at least one human agent;

receiving sensor data associated with relative physical parameters between the at least one robotic agent and the at least one human agent;

modeling the latent states of the at least one human agent as a behavior model by calculating an expected total reward of the at least one human agent for executing an action, wherein the latent states describe cognition of the at least one human agent;

calculating a first belief state based on the relative physical parameters and the behavior model;

predicting future probabilities of future observations at a second time based on the first belief state;

updating a motion plan based on the future probabilities; and

causing the at least one robotic agent to execute the updated motion plan.

15. The non-transitory computer readable storage medium of claim 14 , further comprising:

receiving updated sensor data at the second time;

determining updated relative physical parameters based on the updated sensor data; and

calculating a second belief state based on the updated relative parameters and the behavior model, wherein the future probabilities are based on the second belief state;

predicting updated probabilities of future observations based on the second belief state; and

updating the motion plan based on the updated probabilities.

16. The non-transitory computer readable storage medium of claim 15 , further comprising determining a probe maneuver, wherein the updated sensor data is received in response to the probe maneuver being executed, and wherein the probe maneuver is based on a curiosity reward proportional to an amount of information expected to be gained about the at least one human agent.

17. The non-transitory computer readable storage medium of claim 14 , wherein the expected total reward is calculated using a quantal level reasoning model that includes a rationality coefficient that controls the degree to which the at least one human agent conforms to optimal behaviors.