Navigating Congested Environments with Risk Level Sets

A method is disclosed for use in a planning agent, the method including: identifying a first agent in a vicinity of the planning agent; identifying a location of the first agent and a velocity of the first agent; calculating a set of occupancy costs for the first agent, each occupancy cost in the set of occupancy costs being associated with a different respective location in the vicinity of the planning agent, each occupancy cost in the set of occupancy costs being calculated at least in part based on a cost function that depends on the location of the first agent and the velocity of the first agent; and changing at least one of speed or direction of travel of the planning agent based on the set of occupancy costs.