Planning in mobile robots
A computer-implemented method of determining control actions for controlling a mobile robot comprises: receiving a set of scenario description parameters describing a scenario and a desired goal for the mobile robot therein; in a first constrained optimization stage, applying a first optimizer to determine a first series of control actions that substantially globally optimizes a preliminary cost function for the scenario, the preliminary cost function based on a first computed trajectory of the mobile robot, as computed by applying a preliminary robot dynamics model to the first series of control actions, and in a second constrained optimization stage, applying a second optimizer to determine a second series of control actions that substantially globally optimizes a full cost function for the scenario, the full cost function based on a second computed trajectory of the mobile robot, as computed by applying a full robot dynamics model to the second series of control actions; wherein initialization data of at least one of the first computed trajectory and the first series of control actions is used to initialize the second optimizer for determining the second series of control actions, and wherein the preliminary robot dynamic model approximates the full robot dynamics model, the cost functions embody similar objectives to each encourage achievement of the desired goal, and both are optimized with respect to similar hard constraints, such that the initialization data guides the second optimizer to the substantially globally-optimal second series of control actions.
1. A computer-implemented method of determining control actions for controlling a mobile robot, the method comprising:
receiving a set of scenario description parameters describing a scenario and a desired goal for the mobile robot therein;
in a first constrained optimization stage, applying a first optimizer to determine a first series of control actions that substantially globally optimizes a preliminary cost function for the scenario, wherein the desired goal for the mobile robot is fixed during the first constrained optimization stage, and wherein the preliminary cost function is based on a first computed trajectory of the mobile robot, as computed by applying a preliminary robot dynamics model to the first series of control actions, and
in a second constrained optimization stage, applying a second optimizer to determine a second series of control actions that substantially globally optimizes a full cost function for the scenario, wherein the desired goal for the mobile robot is fixed during the second constrained optimization stage, and wherein the full cost function is based on a second computed trajectory of the mobile robot, as computed by applying a full robot dynamics model to the second series of control actions;
wherein initialization data comprising at least one of the first computed trajectory and the first series of control actions is used to initialize the second optimizer for determining the second series of control actions, and wherein the preliminary robot dynamics model approximates the full robot dynamics model, the cost functions embody similar objectives to each encourage achievement of the desired goal, and both are optimized with respect to similar hard constraints, such that the initialization data guides the second optimizer to the substantially globally-optimal second series of control actions.
2. The method of claim 1 , wherein each computed trajectory is determined, based on an initial mobile robot state, as a series of subsequent mobile robot states;
wherein each mobile robot state of the first computed trajectory is determined by applying the preliminary robot dynamics model to at least a previous mobile robot state of the first computed trajectory and a corresponding control action of the first series of control actions;
wherein each mobile robot state of the second computed trajectory is determined by applying the full robot dynamics model to at least the previous mobile robot state of the second computed trajectory and a corresponding control action of the second series of control actions.
3. The method of claim 2 , wherein the preliminary robot dynamics model is linearly dependent on at least the previous mobile robot state of the first computed trajectory and the corresponding control action of the first series of control actions, and the full robot model is non-linearly dependent on at least one of the previous mobile robot state of the second computed trajectory and the corresponding control action of the second series of control actions.
4. The method of claim 1 , wherein the first optimizer is a mixed integer linear programming (MILP) optimizer, and the second optimizer is a non-linear programming (NLP) optimizer.
5. The method of claim 4 , wherein the hard constraints of the first stage comprise one or more mixed integer collision avoidance constraints for one or more static or moving obstacles in the scenario and/or one or more mixed integer permitted area constraints for keeping the mobile robot within a permitted area, and wherein the hard constraints of the second stage comprise one or more similar collision avoidance and/or permitted area constraints formulated in terms of non-integer variables.
6. The method of claim 1 , wherein the first optimizer applies a receding horizon approximation to iteratively optimize component costs of the preliminary cost function, and thereby determine the first series of control actions, and wherein the second optimizer does not use any receding horizon approximation and instead optimizes the full cost function as a whole.
7. The method of claim 1 , wherein the goal is defined relative to a reference path, and each cost function encourages achievement of the goal by penalizing at least one of lateral deviation from the reference path, and longitudinal deviation from a reference location on the reference path.
8. The method of claim 7 , wherein each of the computed trajectories is represented in a frame of reference defined by the reference path.
9. The method of claim 7 , wherein the preliminary cost function is linearly dependent on said lateral and/or longitudinal deviation, and the full cost function is non-linearly dependent thereon.
10. The method of claim 7 , wherein both cost functions penalize deviation from a target speed.
11. The method of claim 1 , wherein the method is implemented in a planner of a mobile robot and comprises the step of using control data of at least one of: the second computed trajectory, and the second series of control actions to control motion of the mobile robot.
12. The method of claim 1 , wherein the method is performed repeatedly for different scenarios so as to generate:
a first training set comprising inputs to the first optimizer and corresponding outputs computed by the first optimizer, the first training set being used to train a first function approximator to approximate the first optimizer; or
a second training set comprising inputs to the second optimizer and corresponding outputs computed by the second optimizer, the second training set being used to train a second function approximator to approximate the second optimizer; or
a third training set comprising inputs to the first optimizer and corresponding outputs computed by the second optimizer, the third training set being used to train a single function approximator to approximate both of the first and the second optimizers.
13. The method of claim 12 , comprising the step of configuring a runtime stack of mobile robot to implement one of the following:
(i) the first optimizer and the second function approximator, wherein the second function approximator cooperates with the first optimizer within the runtime stack to approximate the second optimization stage;
(ii) the first function approximator and the second optimizer, wherein the first function approximator approximates the first optimization stage for initializing the second optimizer;
(iii) the first function approximator and the second function approximator, which cooperate to approximate both optimization stages; or
(iv) the single function approximator.
14. The method of claim 13 , wherein the runtime stack is configured to implement one of combinations (i) and (ii), or the single function approximator, and the method comprises an additional step of configuring the runtime stack with a verification component configured to verify an output of the second function approximator or the single function approximator.
15. The method of claim 12 , wherein the or each function approximator has a neural network architecture.
16. A computer system comprising one or more hardware processors configured to:
receive a set of scenario description parameters describing a scenario and a desired goal for a mobile robot therein; and
implement or approximate:
a first constrained optimization stage comprising applying a first optimizer to determine a first series of control actions that substantially globally optimizes a preliminary cost function for a scenario, wherein the desired goal for the mobile robot is fixed during the first constrained optimization stage, and wherein the preliminary cost function is based on a first computed trajectory of the mobile robot, as computed by applying a preliminary robot dynamics model to the first series of control actions, and
a second constrained optimization stage comprising applying a second optimizer to determine a second series of control actions that substantially globally optimizes a full cost function for the scenario, wherein the desired goal for the mobile robot is fixed during the second constrained optimization stage, and wherein the full cost function is based on a second computed trajectory of the mobile robot, as computed by applying a full robot dynamics model to the second series of control actions.
17. The computer system of claim 16 , wherein the one or more hardware processors are configured to implement a first optimization component configured to implement or approximate the first optimization stage, and a second optimization component configured to implement or approximate the second optimization stage, using initialization data provided by the first optimization component.
18. The computer system of claim 17 , embodied in a mobile robot, wherein the computer system is further configured to control a motion of the mobile robot via one or more actuators of the mobile robot using control data provided by the second optimization component.
19. The computer system of claim 18 , wherein the mobile robot is an autonomous vehicle.
20. A computer program embodied on non-transitory computer-readable storage media for programming one or more computers to implement the steps of:
receiving a set of scenario description parameters describing a scenario and a desired goal for a mobile robot therein;
in a first constrained optimization stage, applying a first optimizer to determine a first series of control actions that substantially globally optimizes a preliminary cost function for the scenario, wherein the desired goal for the mobile robot is fixed during the first constrained optimization stage, and wherein the preliminary cost function is based on a first computed trajectory of the mobile robot, as computed by applying a preliminary robot dynamics model to the first series of control actions, and
in a second constrained optimization stage, applying a second optimizer to determine a second series of control actions that substantially globally optimizes a full cost function for the scenario, wherein the desired goal for the mobile robot is fixed during the second constrained optimization stage, and wherein the full cost function is based on a second computed trajectory of the mobile robot, as computed by applying a full robot dynamics model to the second series of control actions;
wherein initialization data comprising at least one of the first computed trajectory and the first series of control actions is used to initialize the second optimizer for determining the second series of control actions, and wherein the preliminary robot dynamics model approximates the full robot dynamics model, the cost functions embody similar objectives to each encourage achievement of the desired goal, and both are optimized with respect to similar hard constraints, such that the initialization data guides the second optimizer to the substantially globally-optimal second series of control actions.