EP4078320B1

EP4078320B1 - Planning in mobile robots

Info

Publication number: EP4078320B1
Application number: EP21703178.0A
Authority: EP
Inventors: Majd HAWASLY; Francisco EIRAS; Subramanian Ramamoorthy
Original assignee: Five AI Ltd
Current assignee: Five AI Ltd
Priority date: 2020-01-28
Filing date: 2021-01-28
Publication date: 2024-08-14
Anticipated expiration: 2041-01-28
Also published as: EP4078320A1; US20230081921A1; US12351164B2; WO2021152050A1

Description

Technical Field

The present invention pertains to planning in autonomous vehicles and other mobile robots.

Background

A rapidly emerging technology is autonomous vehicles (AVs) that can navigate by themselves on urban roads. Such vehicles must not only perform complex manoeuvres among people and other vehicles, but they must often do so while guaranteeing stringent constraints on the probability of adverse events occurring, such as collision with these other agents in the environments. An autonomous vehicle, also known as a self-driving vehicle, refers to a vehicle which has a sensor system for monitoring its external environment and a control system that is capable of making and implementing driving decisions automatically using those sensors. This includes in particular the ability to automatically adapt the vehicle's speed and direction of travel based on perception inputs from the sensor system. A fully-autonomous or "driverless" vehicle has sufficient decision-making capability to operate without any input from a human driver. However, the term autonomous vehicle as used herein also applies to semi-autonomous vehicles, which have more limited autonomous decision-making capability and therefore still require a degree of oversight from a human driver. Other mobile robots are being developed, for example for carrying freight supplies in internal and external industrial zones. Such mobile robots would have no people on board and belong to a class of mobile robot termed UAV (unmanned autonomous vehicle). Autonomous air mobile robots (drones) are also being developed.
Recent work in the field has considered planning formulated as a constrained non-linear optimization problem, though mainly in the context of assistive (not fully-autonomous) driving. W. Schwarting, J. Alonso-Mora, L. Paull, S. Karaman, and D. Rus, "Safe nonlinear trajectory generation for parallel autonomy with a dynamic vehicle model," IEEE Transactions on Intelligent Transportation Systems, no. 99, pp. 1-15, 2017, denoted by [1] herein, discloses a "Parallel Autonomy" framework which allows deviation from human driver inputs to maintain safety, but seeks to minimize extreme intervention. A receding horizon planner is formulated as Nonlinear Model Predictive Control (NMPC), subject to a set of hard constraints, namely (1) respecting a transition model of the system, including kinematic and dynamic constraints (2) maintaining the vehicle within the limits of the road and (3) avoiding other traffic participants in the sense of guaranteeing a probability of collision below p_ε . A cost function penalizes deviation from a current acceleration and steering angle specified by a human driver - a form of "soft constraint" or "objective" according to the terminology used herein.
Salvado Joao et al. "Motion planning and goal assignment for robot fleets using trajectory optimisation" describes a scheme for solving the overall fleet management problem in the case of fleets navigating in an environment with obstacles. The method is based on a two phase approach, whereby the first phase solves for fleet-wide boolean decision variables via Mixed Integer Quadratic programming, and the second phase solves for real-valued variables to obtain an optimised set of trajectories for the fleet.
Hult Robert et al. "An MIQP-based heuristic for optimal coordination of vehicles at intersections" proposes a two-stage approximation algorithm to solve the problem of coordinating automated vehicles at intersections. The procedure (a) first solved a Mixed Integer Quadratic Program (MIQP) to compute an approximate solution to the order in which the vehicles cross the intersection; then (b), solves a Nonlinear Program (NLP) for the optimal state and control trajectories.
Most Thomas "Approximation of complex nonlinear functions by means of neural networks" investigates the applicability of neural networks for the approximation of several nonlinear problems. The obtained results are compared to those from classical response surface approaches.

Summary

The techniques of [1] formulate planning as a non-linear constrained optimization problem, and seek to solve that problem using a receding horizon formulation (the solution being a set of control inputs that optimize the cost function). It is possible to extend this framework to more complex cost functions, for example with explicit comfort objectives, or to include more sophisticated vehicle (or, more generally, mobile robot) models. However, a challenge with this approach is that convergence to an optimal set of control inputs is both slow and uncertain, particularly as the complexity of the cost function and/or the mobile robot model increases, i.e. the optimizer may take a long time to converge to a solution, or may never successfully converge. Another challenge is that non-linear constrained optimization solvers tend to be local in nature and thus have a tendency to converge to local optima which may be far from the globally optimal solution. This can significantly impact mobile robot performance.
The present invention also formulates mobile robot planning as a constrained optimization problem. Given a scenario with a desired goal, the problem is to find a series of control actions ("policy") that substantially (exactly or approximately) globally optimises a defined cost function for the scenario and the desired goal. Among other benefits, the present invention addresses the local optima problems that arise in the context of planning based on constrained optimization. This is a form of trajectory planning (synthesis) based on constrained optimization.
A core objective herein is that of reducing instances of convergence to locally but non-globally optimal solutions within a constrained optimization planning framework, thereby improving the robustness of the planning process and the quality of the trajectories it produces. Mere local optima are a particular problem in optimization-based planning, because they typically correspond to low quality trajectories.
To this end, the present invention uses a multi-stage constrained optimisation. A first stage is formulated as an optimization problem that is similar to, but simpler than the more complex planning problem that ultimately needs to be solved. For example, the first stage may use a linear cost function and linear robot dynamics model. Such a problem is generally less susceptible to local optima. The solution of the first stage is then used to initialize the second stage, in which the "full" planning problem is solved. The solution of the first stage will be more likely to be close to a globally optimal solution to the full planning problem - up to up to some acceptable level of error introduced by the simplification assumptions of the first stage - and therefore reduces the tendency of the second stage to converge to local optima far from the global solution.
A first aspect of the invention provides 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 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 is fixed during the second constrained optimisation 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 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.

A less-complex cost function and dynamics model can be used in the first stage, whilst still providing an effective initialization to the second stage. In the present context, the term complexity refers to the form of the cost function and the model, in the space of variables over which they are defined.
A robot dynamics model is a predictive component that predicts how the mobile robot will actually move in the scenario in response to a given sequence of control actions, i.e. it models the mobile robot's response to control actions. A higher-complexity model, as used in the second stage, can model that response more realistically. The lower-complexity model is free to use highly-simplifying assumptions about the behaviour of the mobile robot but these may be relatively unrealistic. Depending on the simplifying assumptions applied in the first stage, the first predicted trajectory may not even be fully dynamically realisable.
A higher-complexity cost function and model, as used in the second stage, can provide superior trajectories, which may be of sufficient quality that they can be used as a basis for effective planning and/or control decisions. However, generally speaking, higher-quality trajectories will be obtained when convergence to an approximately globally optimal solution (i.e. at or near a global optima of the full cost function) is achieved. As the complexity of the full cost function and model increases, achieving such convergence becomes increasingly dependent on the quality of the initialization.
By contrast, the simplifying assumptions applied in the first stage make it inherently less susceptible to the problem of non-local optima, i.e. the ability of the first optimizer to converge to an approximately globally optimal solution is far less dependent on any initialization of the first optimization phase. The output of the simplified first stage is unlikely to be of sufficient quality to use as a basis for such planning decisions directly, and the trajectories it produces may not even be full dynamically realisable (depending on the simplifying assumptions that are made in the first stage). Nevertheless, provided the solution of the first stage is reasonably close to the globally optimal solution of the second stage, the initialization data of the first stage can still facilitate faster and more reliable convergence to an approximately globally optimal solution in the second stage, which will correspond to a dynamically realisable trajectory.
The present invention thus benefits from the high-quality output of the more complex second stage, whilst avoiding (or at least mitigating) the issues of local optima convergence that would otherwise come with it, through the provision of an effective initialization to the second stage.
The described embodiments consider a two-stage constrained optimization. However, other embodiments may use more than two stages. In that case, the first constrained optimization stage that is applied to determine the initialization data could, itself, be a multi-stage optimization. In that case, two or more preliminary cost functions may be optimized in the first stage, with at least one of the preliminary cost functions being optimized in order to initialize another of the preliminary cost functions, before ultimately determining the initialization data to the above-mentioned second constrained optimization stage.
In embodiments, the computed trajectory may be determined, based on an initial mobile robot state, as a series of subsequent mobile robot states. Each mobile robot state of the first computed trajectory may be determined by applying the full robot dynamics model to at least the previous mobile robot state of the first computed trajectory and a corresponding control action of the first series of control actions. Each mobile robot state of the second computed trajectory may be 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.
The preliminary robot dynamics model may be 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 may be 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.
The preliminary cost function may be linearly dependent on the mobile robot states of the first computed trajectory, and the full cost function may be non-linearly dependent on the mobile robot states of the second computed trajectory.
The preliminary cost function may be linearly dependent on the control actions of the first series, and the full cost function may be non-linearly dependent on the control actions of the second series.
The first optimizer may be a mixed integer linear programming (MILP) optimizer, and the second optimizer may be a non-linear programming (NLP) optimizer.
The hard constraints of the first stage may 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. The hard constraints of the second stage may comprise one or more similar collision avoidance and/or permitted area constraints formulated in terms of non-integer variables.
The first optimizer may apply a receding horizon approximation to iteratively optimize component costs of the preliminary cost function, and thereby determine the first series of control actions, and the second optimizer may not use any receding horizon approximation and may instead optimize the full loss function as a whole.
The goal may be defined relative to a reference path, and each cost function may encourage achievement of the goal by penalizing at least one of lateral deviation from the deference path, and longitudinal deviation from a reference location on the reference path.
Each of the computed trajectories may be represented in a frame of reference defined by the reference path.
The preliminary cost function may be linearly dependent on the above lateral and/or longitudinal deviation, and the full cost function is non-linearly dependent thereon.
Both cost functions may penalize deviation from a target speed.
The method may be implemented in a planner of a mobile robot and comprise 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.
Embodiments and optional implementations of the invention address the problem of speed though the use of function approximation. Depending on the resources of the available hardware platform, there may be occasions when running the first and second optimizations in real-time is not feasible. As an optional optimization, one or both of the optimizers may be replaced, in a real-time context, with a function approximator training to approximate the first and/or second optimization stage as applicable.
For example, the method may be 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, and the training set may be used to train a first function approximator to approximate the first optimizer (training method 1).
Alternatively or additionally, the method may be performed repeatedly for different scenarios so as to generate a second training set comprising inputs to the second optimizer and corresponding outputs computed by the second optimizer, and the training set may be used to train a second function approximator to approximate the second optimizer (training method 2).
The method may comprise the step of configuring a runtime stack of mobile robot to implement one of the following combinations:

(i) the first optimiser and the second function approximator, wherein the second function approximator cooperates with the first optimiser within the run-time 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; and
(iii) the first function approximator and the second function approximator, which cooperate to approximate both optimization stages.

Alternatively, the method may be performed repeatedly for different scenarios so as to generate a training set comprising inputs to the first optimizer and corresponding outputs computed by the second optimizer, and the training set may be used to train a single function approximator to approximate both of the first and the second optimizers (training method 3).
The method may comprise the step of configuring a runtime stack of mobile robot to implement the single function approximator.
The runtime stack may be configured to implement one of combinations (i) and (ii), or the single function approximator, and the method may comprise 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.
A further embodiment of the invention provides 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;
in a first constrained optimization stage, applying a first optimizer to determine a first series of control actions that substantially optimize a lower-complexity cost function for the scenario, wherein the desired goal is fixed during the first constrained optimization stage, and wherein the lower-complexity cost function is dependent on a first computed trajectory of the mobile robot, as computed by applying a lower-complexity 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 optimize a higher-complexity cost function for the scenario, wherein the desired goal is fixed during the second constrained optimisation stage, and wherein the higher-complexity cost function is dependent on a second computed trajectory of the mobile robot, as computed by applying a higher-complexity robot dynamics model to the second series of control actions;
wherein the lower-complexity robot dynamic model approximates the higher-complexity robot dynamics model, wherein the cost functions embody similar objectives to each encourage achievement of a desired goal in the scenario, and both are optimized with respect to similar hard constraints, such that the first computed trajectory approximates the second computed trajectory, and 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.

A further aspect herein provides a computer system comprising one or more optimization components configured to implement or approximate: a first optimization stage as defined in any of the above aspects or embodiments; and a second optimization stage as defined in any of the above aspects or embodiments.
The one or more optimization components may comprise 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.
The first optimization component may take the form of a first function approximator trained in accordance with training method 1 above and/or the second optimization component may take the form of a second function approximator trained in accordance with training method 2 above to approximate the second optimization stage.
The second optimization component may take the form of a second function approximator, and the computer system may additionally comprise a verification component configured to verify at least one of a second trajectory and a second series of control actions computed by the second function approximator.
The one or more optimization components may comprise a single optimization component, trained in accordance with training method 3 above, to approximate both of the first and second optimization stages.
The computer system may additionally comprise a verification component configured to verify at least one of a second trajectory and a second series of control actions computed by the single function approximator.
The or each function approximator may have a neural network architecture.
The computer system may be embodied in an autonomous vehicle or other mobile robot, wherein the computer system may be further configured to control the motion of the mobile robot via one or more actuators of the mobile robot using control data provided by the second optimization component.
A further aspect herein provides a computer program for programming one or more computers to implement any of the methods disclosed herein.

Brief Description of Figures

To assist understanding of the present disclosure and to show how embodiments of the present invention may be put into effect, reference is made by way of example to the accompanying drawings, in which:

Figure 1 shows a schematic block diagram of an autonomous vehicle runtime stack;
Figure 2A shows mathematical transformations to and from a reference-path based frame of reference;
Figure 2B geometrically depicts a representation from a global frame of reference into a reference path-based frame of reference;
Figure 3 shows a two-stage constrained optimization architecture;
Figure 4 schematically depicts an example trajectory;
Figure 5 shows a comparison of example trajectories in first and second optimization stages respectively;
Figure 6 shows how a function approximator may be trained to approximate a NLP constrained optimization stage;
Figure 7 shows how a function approximator may be trained to approximate a MILP constrained optimization stage;
Figure 8 shows an online planner comprising first and second function optimizers which have been trained to approximate a two-stage constrained optimization process;
Figure 9 shows a single function approximator trained to approximate a multi-stage constrained optimization process;
Figure 10 shows further examples of driving scenarios;
Figure 11 shows an example of a plan computed for a first example driving scenario;
Figure 12 shows an example of a plan computed for a second example driving scenario;
Figure 13 shows an example of a residential driving scenario; and
Figure 14 shows a plot summarizing a set of results that demonstrate the efficacy of initializations generated using the present techniques on different scenarios.

Detailed Description

I. EXAMPLE CONTEXT

Embodiments are described in the context of an autonomous vehicle. However, the description applies equally to other forms of autonomous mobile robot.
Figure 1 shows a highly schematic block diagram of a runtime stack 100 for an autonomous vehicle (AV). The run time stack 100 is shown to comprise a perception stack 102, a prediction stack 104, a planner 106 and a controller 108.
The perception stack 102 receives sensor outputs from an on-board sensor system 110 of the AV.
The on-board sensor system 110 can take different forms but generally comprises a variety of sensors such as image capture devices (cameras/optical sensors), LiDAR and/or RADAR unit(s), satellite-positioning sensor(s) (GPS etc.), motion sensor(s) (accelerometers, gyroscopes etc.) etc., which collectively provide rich sensor data from which it is possible to extract detailed information about the surrounding environment and the state of the AV and any external actors (vehicles, pedestrians, cyclists etc.) within that environment.
Hence, the sensor outputs typically comprise sensor data of multiple sensor modalities such as stereo images from one or more stereo optical sensors, LiDAR, RADAR etc.
Stereo imaging may be used to collect dense depth data, with LiDAR/RADAR etc. proving potentially more accurate but less dense depth data. More generally, depth data collection from multiple sensor modalities may be combined in a way that respects their respective levels (e.g. using Bayesian or non-Bayesian processing or some other statistical process etc.). Multiple stereo pairs of optical sensors may be located around the vehicle e.g. to provide full 360° depth perception. This provides a much richer source of information than is used in conventional cruise control systems.
The perception stack 102 comprises multiple perception components which co-operate to interpret the sensor outputs and thereby provide perception outputs to the prediction stack 104.
The perception outputs from the perception stack 102 are used by the prediction stack 104 to predict future behaviour of the external actors.
Predictions computed by the prediction stack 104 are provided to the planner 106, which uses the predictions to make autonomous driving decisions to be executed by the AV in a way that takes into account the predicted behaviour of the external actors.
The planner 106 implements the techniques described below to plan trajectories for the AV and determine control actions for realizing such trajectories. In particular, a core function of the planner 106 is to determine a series of control actions for controlling the AV to implement a desired goal in a given scenario. In a real-time planning context, a scenario is determined using the perception stack 102 but can also incorporate predictions about other actors generated by the prediction stack 104. A scenario is represented as a set of scenario description parameters used by the planner 106. A typical scenario would define a drivable area and would also capture predicted movements of any obstacles within the drivable area (such as other vehicles) along with a goal. A goal would be defined within the scenario, and a trajectory would then need to be planned for that goal within that scenario. In the following, obstacles are represented probabilistically in a way that reflects the level of uncertainty in their perception within the perception stack 102.
The goal could for example be to enter a roundabout and leave it at a desired exit; to overtake a vehicle in front; or to stay in a current lane at a target speed (lane following). The goal may, for example, be determined by an autonomous route planner (not shown).
The controller 108 executes the decisions taken by the planner 106 by providing suitable control signals to on-board actuators 112 such as motors of the AV. In particular, the controller 108 controls the actuators in order to control the autonomous vehicle to follow a trajectory computed by the planner 106.
As described in further detail below, the planner 106 plans over acceleration (magnitude) and steering angle control actions simultaneously, which are mapped to a corresponding trajectory by modelling the response of the vehicle to those control actions. This allows constraints to be imposed both on the control actions (such as limiting acceleration and steering angle) and the trajectory (such as collision-avoidance constraints), and ensures that the final trajectories produced are dynamically realisable. The planner 106 will determine an optimal trajectory and a corresponding sequence of control actions that would result in the optimal trajectory according to whatever vehicle dynamics model is being applied. The control actions determined by the planner 106 will not necessarily be in a form that can be applied directly by the controller 108 (they may or may not be). Ultimately, the role of the planner 106 is to plan a trajectory and the role of the controller 108 is to implement that trajectory. The term "control data" is used herein to mean any trajectory information derived from one of both of the planned trajectory and the corresponding series of control actions that can be used by the controller 108 to realize the planner's chosen trajectory. For example, the controller 108 may take the trajectory computed by the planner 106 and determine its own control strategy for realizing that trajectory, rather than operating on the planner's determined control actions directly (in that event, the controller's control strategy will generally mirror the control actions determined by the planner, but need not do so exactly).
The planner 106 will continue to update the planned trajectory as a scenario develops. Hence, a trajectory determined at any time may never be realized in full, because it will have been updated before then to account for changes in the scenario that were not predicted perfectly.
These functions of the planner 106 will now be described in detail. As set out above, the present disclosure addresses the planning problem of determining an optimal series of control actions (referred to herein as a "policy") for a given scenario and a given goal as a constrained optimization problem with two optimization stages.
In the described embodiments, the first optimization stage solves a Mixed Integer Linear Programming (MILP) problem where obstacles lead to hard constraints for the MILP. The outcome of this seeds (initializes) a subsequent nonlinear optimisation of the trajectory for dynamics and comfort constraints, in the second optimization stage. The second optimization stage solves a Non-Linear Programming (NLP) problem, initializing using the results of the MILP optimization stage. The MILP stage uses a mixed-integer formulation of collision avoidance and drivable area constraints as described later.
Although performed second, the NLP stage is described first, in order to set out the final planning problem that ultimately needs to be solved (Problem 2 below), and to provide some context for the subsequent description of how best to provide an effective initialization (seed) from the MILP stage.
Before describing either stage in detail, the description sets out a general framework applicable to both stages.

II. PRELIMINARIES AND PROBLEM STATEMENT

A. Notation and Definitions

Following notation from [1], an index k ≡ t_k, is assumed where t_k = t₀ + kΔt with t₀ being the current time and Δt a fixed timestep. A trajectory is defined over the next N steps covering a temporal horizon of τ = NΔt. Vectors are in bold. A shorthand notation r_i:e = {r_i, ..., r_e } is used for any variable r.
A vehicle whose motion is being planned is referred to as the ego vehicle. The ego vehicle's state at time k is given by X _k =(X_k, Y_k, V_k, Φ_k ) ∈
, where (X_k, Y_k ) is the position of the vehicle, V_k is the speed and Φ_k is its heading in a global coordinate frame
. The ego state at time 0 is given by X ₀, and a function of the planner 106 is to determine the ego states over the next N steps,
Other traffic participants, such as vehicles, pedestrians and cyclists, are indexed by i ∈ {1, ..., n} and their pose at time k is modelled using a Gaussian distribution having a mean
and covariance $Γ_{k}^{i}$
. The set of the mean poses of all traffic participants over time is $O_{0 : N}^{1 : n}$
, and similarly for the covariances $Γ_{0 : N}^{1 : n}$
, and both are given as inputs to the planning problem.

B. Reference Path-based Representation

To simplify the process of defining a planning goal, the global coordinate frame is transformed to a reference path-based representation under an invertible transform
as described in Sec. II-B. This representation significantly simplifies the problem of path tracking.
A goal of the ego vehicle is defined as following a differentiable and bounded two-dimensional reference path in the global coordinate frame,
, parameterized by the distance from the start of the path (
). Tangential and normal vectors of the reference path in the global coordinate frame can be obtained at any point λ along the path
as $t_{λ} = [\begin{matrix} \frac{\partial X^{P_{ref}} (λ)}{\partial λ} \\ \frac{\partial Y^{P_{ref}} (λ)}{\partial λ} \end{matrix}], n_{λ} = [\begin{matrix} \frac{- \partial Y^{P_{ref}} (λ)}{\partial λ} \\ \frac{\partial X^{P_{ref}} (λ)}{\partial λ} \end{matrix}]$
respectively.
The reference path
is a path which the ego vehicle is generally intending to follow, at a set target speed. However, deviation from the reference path and target speed, whilst discouraged, are permitted provided that no hard constraints (such as collision avoidance constraints) are breached. The reference path can be determined using knowledge of the road layout, which may use predetermined map data (such as an HD map of the driving area), information from the perception stack 104, or a combination of both. For complex layouts in particular (such as complex junctions or roundabouts), the reference path could be learned by monitoring the behaviour of other drivers in the area over time.
Figure 2A illustrates the process of going from an input (X ₀, $O_{0 : N}^{1 : n}, Γ_{0 : N}^{1 : n}$
) to the desired output X _1:N. Given an input (X ₀, $O_{0 : N}^{1 : n}, Γ_{0 : N}^{1 : n}$
) (upper case) the invertible transformation
is applied to yield (x ₀, $o_{0 : N}^{1 : n}, Σ_{0 : N}^{1 : n}$
) (lower case) in the reference path-based representation, which the planner 106 then uses to plan. The output of the planner 106 is an intended trajectory x _0:N in the reference path-based representation, which in turn is transformed back to the global coordinate frame by applying the inverse transformation
to obtain X _0:N in the global coordinate frame. Further details of the transformation are described below.
The invertible transform
operates over three types of input: (1) poses, (2) velocities and (3) covariance matrices. Each of the individual operations is described next.

1) Pose transform:
maps poses (X, Y, Φ) in the global coordinate frame
to poses (x, y, φ) in the reference path frame
as shown in Figure 2B.
- x =
  [X Y] is the distance of the projection of [X Y] to
  from the beginning of the path, defined as the solution to the following optimization problem: $\underset{λ}{argmin} {(X - X^{P_{ref}} (λ) ())}^{2} + {(Y - Y^{P_{ref}} (λ) ())}^{2} .$
  Due to the nature of the optimization, no closed-form solution can be obtained for x.
- $y = \frac{1}{‖ n_{x} ‖} n_{x}^{T} \cdot \hat{y}$
  , where n _x is the normal vector of the reference path at λ = x as in (1), and $\hat{y} = [\begin{matrix} X - X^{P_{ref}} (x) \\ Y - Y^{P_{ref}} (x) \end{matrix}]$
  .
- φ = ∠t _x - Φ, where $∠ t_{x} = \arctan ({\frac{\partial Y^{P_{ref}} (λ)}{\partial X^{P_{ref}} (λ)} |}_{λ = x})$

Figure 2B shows a visual representation of the transform
between the world frame of reference and the reference path-based frame, and
, the inverse transform.
As will be appreciated, the inverse transformation
can be derived straightforwardly by applying the same geometric principles.

2) Velocity transform: since
is defined spatially, speeds are invariant to it: v =
(V) = V.
3) Covariance transform: considering a traffic participant with pose O and covariance Γ, such that
(O) = [x y φ] ^T, the transformed covariance matrix in the reference path coordinate frame is given by: $Σ = (Γ) = R (∠ t_{x} - ϕ) Γ R {(∠ t_{x} - ϕ)}^{T}$
where t _x is the tangent of
evaluated at λ = x, and R E SO(2) is a rotation matrix.

C. Problem Statement

Following the reasoning presented in Sec. II-A, the planning problem is defined in the reference path coordinate frame.
The state of the ego vehicle at time k is given by x _k = (x_k ,y_k,v_k ,φ_k ) ∈
where (x_k ,y_k ) is the 2D position of the vehicle, v_k is its speed and φ_k is its heading. The evolution of the vehicle state is given by a discrete general dynamical system: $x_{k + 1} = ƒ_{Δ t} (x_{k}, u_{k}),$
where f _Δt is a discrete non-linear function with time parameter Δt, and u _k = (a_k ,δ_k ) ∈
are an acceleration and steering angle, respectively, applied at time k. The ego vehicle is modelled as a rigid body occupying an area $S_{e} \subset ℝ^{2}$
relative to its center, and the area occupied by the ego vehicle at state x _k is given by
For other traffic participants, pose at time k is described probabilistically with mean $o_{k}^{i} =$
$(x_{k}^{i}, y_{k}^{i}, ϕ_{k}^{i}) \in O$
and covariance $Σ_{k}^{i}$
, for i ∈ {1, ..., n}. Following the definition from [1], the area each traffic participant occupies is defined as
with probability larger than p_ε .
A driveable surface area $B \subset ℝ^{2}$
is defined as the area in which it is safe for the ego vehicle to drive, with the unsafe area $B_{out} = ℝ^{2} \ B$
. With a cost function $J (x_{0 : N}, u_{0 : N - 1}, o_{0 : N}^{1 : n}, Σ_{0 : N}^{1 : n})$
defined over the positions and controls of the ego vehicle and states and uncertainties of other traffic participants, a "policy synthesis" problem can be formulated.
As indicated above, a "policy" in the present context refers to a time-series of control actions, which is mapped to a corresponding vehicle trajectory using a vehicle dynamics model.
Problem 1 (Policy Synthesis). Given an initial ego state x₀ , and trajectories of other traffic participants ( $o_{0 : N}^{1 : n}, Σ_{0 : N}^{1 : n}$
), compute an optimal policy: $\begin{array}{l} u_{0 : N - 1}^{*} = & \underset{u_{0 : N - 1}}{argmin} & J (x_{0 : N}, u_{0 : N - 1}, o_{0 : N}^{1 : n}, Σ_{0 : N}^{1 : n}) \\ s . t . & \forall k \in \{0, \dots, N\} : \\ x_{k + 1} = ƒ_{Δ t} (x_{k}, u_{k}) \\ S (x_{k}) \cap B_{out} = \emptyset \\ S (x_{k}) \cap [\underset{i \in \{1, \dots, n\}}{\cup} S^{i} (o_{k}^{i}, Σ_{k}^{i}, p_{ϵ})] = \emptyset \end{array}$

III. NON-LINEAR PROGRAMMING FORMULATION

This section describes a specific solution to Problem 1, posing it as a Non-Linear Programming (NLP) problem with a set of hard constraints and a multi-objective cost function. The hard constraints comprise (1) vehicle model constraints, including kinematic ones on the transitions of the model and on the controls it allows (Sec. III-1); and (2) collision avoidance constraints with the purpose of avoiding collisions with the boundaries of the road (which can include, e.g. construction areas or parked vehicles) as well as other traffic participants (Sec. III-2). Soft constraints make up the terms of the cost function (i.e. are embodied in the cost function itself) and are presented in Sec. III-3, whereas hard constraints constrain the optimization of the cost function. Sec. III-4 describes the full problem.

1) Vehicle Model: A "kinematic bicycle" model is applied, which is a simplified car model focused on the center of the vehicle. Assuming the ego vehicle to be a rectangle with an inter-axel distance of L, this model can be discretized under Δt as: $[\begin{matrix} x_{k + 1} \\ y_{k + 1} \\ ϕ_{k + 1} \\ υ_{k + 1} \end{matrix}] = [\begin{matrix} x_{k} \\ y_{k} \\ ϕ_{k} \\ υ_{k} \end{matrix}] + [\begin{matrix} υ_{k} \cos (ϕ_{k} + δ_{k}) \\ υ_{k} \sin (ϕ_{k} + δ_{k}) \\ \frac{2 υ_{k}}{L} \sin (δ_{k}) \\ a_{k} \end{matrix}] Δ t$

While the kinematic bicycle model is not strictly limited by any specific values of maximum steering and acceleration, a limit is imposed on the allowed ranges of controls by enforcing | δ_k | ≤ δ _max and a _min ≤ a_k ≤ a _max, as well as limiting jerk |a _k+1 - a_k | ≤ ȧ _max and angular jerk |δ _k+1 - δ_k | ≤ δ̇_max. These maintain the model within the operational domain, and also help to ensure that the resulting trajectories respect the rules of the road and passenger comfort. Finally, speed is constrained as 0 ≤ v _min ≤ v_k ≤ v _max, to guarantee forward motion within a set speed limit v _max.
The use of the same positive and negative jerk magnitude mirrors has been observed to work in practice, however the framework can be extended to use different jerk bounds.
2) Collision Avoidance: The collision avoidance problem is divided into two different types of avoidance: the limits of the road (which may include other features such as construction zones or parked vehicles) and other traffic participants. For both, a representation of the area of the ego-vehicle is defined as
(x _k ) for state x _k. A simplified representation is used wherein the ego vehicle boundary is represented solely by its corners. For a rectangular vehicle of width w and length l, the positions of the corners can be defined as a function of the state x _k as: $c^{z} (x_{k}) = [\begin{matrix} x_{k}^{z} \\ y_{k}^{z} \end{matrix}] = R (ϕ_{k}) (z^{T} \circ [\begin{matrix} w / 2 \\ l / 2 \end{matrix}]) + [\begin{matrix} x_{k} \\ y_{k} \end{matrix}]$
where z ∈ Z = {[11],[-11],[-1-1],[1-1]}, R(φ_k ) ∈ SO(2) is the rotation matrix corresponding to the ego-vehicle's heading, and ∘ is the Hadamard product. Following this definition, the maintenance of the ego-vehicle within the driveable surface, corresponding to the constraint defined as
(x_k ) ∩
= Ø, can be reduced to the constraint
The driveable surface
is described by limits, b_l (λ), b_r (λ), which are continuous and differentiable functions of λ defining the left and right borders, respectively, with ∀λ: b_l (λ) < b_r (λ). These boundaries impose a constraint on the allowed lateral deviation at position x, such that keeping the ego-vehicle within the driveable surface,
(x_k ) ∩ B_out = Ø, can be reduced to the set of constraints
with ${[x_{k}^{z}, y_{k}^{z}]}^{T}$
defined as in (6).
For other traffic participants, the area occupied
by each participant is modelled in a similar fashion to [1], with the exception that the uncertainty at time k can be a non-diagonal matrix Σ _k for the purpose of the soft constraints introduced next in Sec. III-3.
However, for the purposes of illustration, a diagonal matrix $Σ_{k}^{D}$
is considered to obtain: $S^{i} (o_{k}^{i}, Σ_{k}^{i}, p_{ϵ}) \subset L (a_{Σ_{k}^{D}} + a_{shape}, b_{Σ_{k}^{D}} + b_{shape}) = L (a_{k}^{i}, b_{k}^{i})$
where $L (a_{k}^{i}, b_{k}^{i})$
is an ellipse which conservatively inscribes vehicle i at time k up to an uncertainty p_ε . Thus, the set of constraints can be expressed as:
$g^{i, z} (x_{k}) = {[\begin{matrix} x_{k}^{z} - x_{k}^{i} \\ y_{k}^{z} - y_{k}^{i} \end{matrix}]}^{T} R {(ϕ_{k}^{i})}^{T} [\begin{matrix} \frac{1}{{(a_{k}^{i})}^{2}} & 0 \\ 0 & \frac{1}{{(b_{k}^{i})}^{2}} \end{matrix}] R (ϕ_{k}^{i}) [\begin{matrix} x_{k}^{z} - x_{k}^{i} \\ y_{k}^{z} - y_{k}^{i} \end{matrix}] .$
3) Cost function: The multi-objective cost function is defined for a set of soft constraints
over the states and controls of the ego, as well as the positions and uncertainty of the other traffic participants. If a soft constraint
∈
is defined via a function $θ_{ι} (x, u, o^{1 : n}, Σ^{1 : n})$
, and a weight $ω_{ι} \in ℝ^{+}$
, the cost function can be defined as: $J (x_{0 : N}, u_{0 : N - 1}, o_{0 : N}^{1 : n}, Σ_{0 : N}^{1 : n}) = \sum_{k = 0}^{N} J_{I}^{k} (x_{k}, u_{k}, o_{k}^{1 : n}, Σ_{k}^{1 : n})$
where: $J_{I}^{k} (x, u, o^{1 : n}, Σ^{1 : n}) = \sum_{ι \in I} ω_{ι} θ_{ι} (x, u, o^{1 : n}, Σ^{1 : n})$
The weights associated with the soft constraints determine their relative importance. Soft constraints are defined over different objectives:

Progress over a longitudinal goal x_g : θ_x = (x - x_g )², a target speed v_g : θ_v = (v - v_g )² , or for lane tracking under the reference path representation, θ_y = y ² .
Comfort as a minimization of the norm of acceleration, θ_a = a ², and steering, θ_δ = δ ².
Calculated Risk, defined based on the uncertainty of the other vehicles assuming $p^{i} = [x - x^{i}, y - y^{i}], θ_{Σ} = \sum_{i = 1}^{n} \frac{1}{|Σ^{i}|} \exp (- {(p^{i})}^{T} {(Σ^{i})}^{- 1} (p^{i})) .$

4) NLP formulation: The optimization problem is formulated using the constraints
and cost J.
Problem 2 (Non-Linear Programming Problem). Given an initial ego state x₀ , and trajectories of other traffic participants ( $o_{0 : N}^{1 : n}, Σ_{0 : N}^{1 : n}$
), and the set of soft constraints J, compute the optimal policy:
Due to the non-linearity of J, f_Δt, b_l, b_r and g^i,z , this problem is a general (equality and inequality constraints) non-linear, non-convex, constrained optimization problem. While it is appealing to solve the problem directly, or using a receding horizon formulation as in [1], there are two major challenges to this approach:

Slow and uncertain convergence: solvers for these types of problems are generally slow for large instances, such as Problem 2, particularly when initialization is not carefully considered. While advances in efficient primal-dual interior point solvers have mitigated this issue to a certain degree, the convergence to a solution is uncertain.
Local optima: non-linear constrained optimization solvers tend to be local in nature, finding solutions close to the initial point and, possibly, far from the global optimum.

In the above, the tuple (x ₀, $O_{0 : n}^{1 : N}, Σ_{0 : n}^{1 : N}$
) is an example of a set of scenario description parameters, describing a dynamic scenario and a starting state of the ego vehicle within it. The dynamics of the scenario is captured in terms of the motion components of x ₀, $O_{0 : n}^{1 : N}$
and $Σ_{0 : n}^{1 : N},$
, i.e. the speed and heading dimensions within the NLP formulation.

IV. TWO-STEP OPTIMIZATION

To mitigate these NLP issues, a framework depicted schematically in Figure 3 is used.
Figure 3 shows a two-tier optimization architecture: a first stage 302 corresponds to solving a receding horizon formulation of the linearized version of the problem; in a second stage 304, the problem is solved in a non-linear non-convex constrained optimization fashion using the solution of the first stage as an initialization (e.g. a warm-start initialization).
The two stage optimisation of Figure 3 may be referred to as 2s-OPT.
A motivation behind the architecture is to avoid the local optima convergence in the non-linear, non-convex constrained optimization by providing an initial solution that is closer to the global optimum, which also should lead to faster and more reliable convergence.
In the two-tier optimization, the first stage 302 solves a linearized version of Problem 2 in a finite, receding horizon manner using a Mixed Integer Linear Programming (MILP) formulation (details presented in Sec. IV-A). This gives guarantees on the optimality for each stage of the receding horizon problem (see Sec. IV-A5), and, thus, acts as a proxy towards reaching the global optimum of the linearized problem. The second stage 304 uses the output of the MILP optimizer as an initial solution and solves the full Problem 2, as set out above. Given the similar representations of the linearized and non-linear problems, it is expected that this initialization improves convergence, speed and the quality of the final solution.

A. Mixed Integer Linear Programming Formulation

In this stage, Problem 2 is re-formulated as a Mixed Integer Linear Programming optimization problem, with mixed integer linear constraints and a multi-objective cost function.
To do so, a linear vehicle model with kinematic feasibility constraints is considered (Sec. IV-A1). An approach to collision avoidance is formulated which maintains the mixed integer linearity of the model (Sec. IV-A2). In Sec. IV-A3 an interpretation of the soft constraints within the MILP cost function is provided. The full MILP problem in Sec. IV-A4 and discuss the optimality of the solution in Sec. IV-A5.
The following sections make use of the non-linear operators | • |, max(•) and min(•), which can be enforced in MILP by considering auxiliary binary variables under the "big-M" formulation [8], [9]. For example, for a constraint C = max(A,B), assume b 6 {0,1} to be a binary variable and $M \in ℝ^{+}$
to be a sufficiently large value. It is then possible to define the corresponding mixed integer constraints: $\begin{array}{l} C \geq A \\ C \geq B \\ C \leq A + Mb \\ C \leq B + M (1 - b) \end{array}$
Similar definitions can be obtained for the operators | • | and min.

I) Linear Vehicle Model and Kinematic Feasibility: The kinematic bicycle model presented in Sec. Ill-1 is non-linear, but can be linearized around a point using a series expansion. The problem with this approach lies on the fact that this approximation is only valid around the point, yielding higher errors as the distance to the point increases. To avoid this issue, a different vehicle model with nonholonomic constraints is adopted for the first stage 302.

A state of this linear vehicle model at time k is defined as
where
is the whole state-space for the MILP formulation, with: $υ_{k}^{x} = υ_{k} \cos (ϕ_{k}), υ_{k}^{y} = υ_{k} \sin (ϕ_{k})$
and with controls ${\overline{u}}_{k} = [\begin{matrix} a_{k}^{x} & a_{k}^{y} \end{matrix}] \in U^{M}$
, where
is the input space for the MILP formulation.
This representation is still based on the reference path
, and is the same as the representation used in the second stage 304 in terms of the spatial dimensions. However, whereas the second stage 304 uses an angular representation of motion (i.e. velocity and acceleration are represented in terms of magnitude and angle components), the first stage uses a linear representation of velocity and acceleration.
The linear vehicle dynamics model is defined as: ${\overline{x}}_{k + 1} = F_{Δ t} ({\overline{x}}_{k}, {\overline{u}}_{k}) = A_{d} {\overline{x}}_{k} + B_{d} {\overline{u}}_{k}$
where F _Δt corresponds to a zero-order hold discretization of the continuous state-space system: $\dot{\overline{x}} = [\begin{matrix} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{matrix}] {\overline{x}}^{T} + [\begin{array}{l} 0 & 0 \\ 0 & 0 \\ 1 & 0 \\ 0 & 1 \end{array}] {\overline{u}}^{T}$
This nonholonomic model is highly simplified when compared to the kinematic bicycle model in (5). To introduce kinematic feasibility, the following constraint is imposed: $υ^{x} \geq ρ |υ^{y}|$
for a given constant ${ρεℝ}^{+}$
. Assuming forward motion, that is $ϕ \in [- \frac{π}{2}, \frac{π}{2}]$
, this constraint dictates that any movement in the y direction requires motion in the x direction as well.
Similar constraints are imposed as in the non-linear model of the second stage 304, in particular input bounds constraints, a^x _min ≤ a^x _k ≤ a^x _max and a^y _min ≤ a^y _k ≤ a^y _max ; jerk related constraints, $|a_{k + 1}^{x} - a_{k}^{x}| < Δ a_{\max}^{x} Δ t$
and $|a_{k + 1}^{y} - a_{k}^{y}| < Δ a_{\max}^{y} Δ t$
; and velocity constraints v^x _min ≤ v^x _k ≤ v^x _max and v^y _min ≤ v^y _k ≤ v^y _max, with v^x _min ≥ 0 to guarantee forwards motion.
2) Collision Avoidance: Similarly to the non-linear formulation, collision avoidance is split into avoiding the limits of the road and avoiding other traffic participants. However, a key difference is that the state x does not explicitly model the orientation of the vehicle, so linearly approximating the calculation of the corners of the ego vehicle would induce very high errors in the model. Thus, the ego vehicle is considered to be a point p _k = [x_k, y_k ] and deal with the area it occupies in the definition of the road limits and other traffic participants. For the driveable space, piecewise-linear functions
and
are defined for the left and right road boundaries respectively, such that
To take the size of the ego vehicle into account, the constraint is formulated as: $d + b_{l}^{M} (x_{k}) \leq y_{k} \leq b_{r}^{M} (x_{k}) - d$
where d :
is a function of the size of the ego vehicle with respect to its point estimate. In the most conservative case, assuming a rectangular ego vehicle of width w and length l, it is possible to define $d = \sqrt{(w^{2} + l^{2})} / 2$
, i.e. reduce the driveable surface to
where ⊕ is the Minkowski-sum operator. For practical purposes, d = w/2 is considered, which is exact when φ = 0.
For traffic participants, the ellipses $L (a_{k}^{i}, b_{k}^{i})$
defined in (9) are inscribed with axis-aligned rectangles, which are then augmented to consider the point estimate of the ego vehicle's pose. With d^x and d^y being functions of the size of the ego vehicle with respect to its center in the x and y direction, respectively, rectangles $R_{k}^{i}$
are defined with the limits: $\begin{array}{l} x_{k, \min}^{i} = [\min_{x} L (a_{k}^{i}, b_{k}^{i})] - d^{x} \\ x_{k, \max}^{i} = [\max_{x} L (a_{k}^{i}, b_{k}^{i})] + d^{x} \\ y_{k, \min}^{i} = [\min_{y} L (a_{k}^{i}, b_{k}^{i})] - d^{y} \\ y_{k, \max}^{i} = [\max_{y} L (a_{k}^{i}, b_{k}^{i})] + d^{y} . \end{array}$
It should be noted that $x_{k, \min}^{i}, x_{k, \max}^{i}, y_{k, \min}^{i}, y_{k, \max}^{i}$
can be computed in closed form from $L (a_{k}^{i}, b_{k}^{i})$
, d^x and d^y.
Then, the collision avoidance constraint is the logical implication: $x_{k, \min}^{i} \leq x \leq x_{k, \max}^{i} \land y \geq y_{k, \min}^{i} \Rightarrow y \geq y_{k, \max}^{i}$
which can be understood as "if the ego position is aligned with the vehicle along x, then it must be outside the vehicle's borders in y". The big-M formulation [8], [9] is used to obtain the following mixed integer constraint: $\begin{array}{r} y_{k, \max}^{i} - {Mμ}_{k}^{i} \leq y_{k}, \\ μ_{k}^{i} = \max (x_{k, \min}^{i} - x_{k}, 0) + \max (x_{k} - x_{k, \max}^{i}, 0) \\ + \max (y_{k, \min}^{i} - y_{k}, 0) \end{array}$
for a sufficiently large $M \in ℝ^{+}$
.
3) Mixed Integer Linear Cost Function: For best performance, the cost function of the MILP stage 302 should be similar to the cost function from the non-linear stage 304 to minimize the gap between the optimum obtained in both stages.
If the cost functions are similar, and subject to similar constraints, then the optimal trajectory computed in the first stage 302 should approximate the final optimal trajectory computed in the second stage 304, and therefore provide a useful initialization.
The MILP problem is defined in a receding horizon formulation, in which the cost over each state (individual cost) is also defined independently in time, with: $\begin{array}{r} J_{C}^{M, k} (\overline{x}, \overline{u}, o^{1 : n}, Σ^{1 : n}) = \sum_{ι \in C} Ω_{ι} Θ_{ι} (\overline{x}, \overline{u}, o^{1 : n}, Σ^{1 : n}), \\ k \in \{0, \dots, N\} \end{array}$
for a set of soft constraints C, where each constraint
is defined by a function
( x, u, o^1:n, Σ^1:n ) and its weight
The weights determine the relative importance of each constraint and can be fine-tuned to optimize performance.
Similarly to the non-linear stage 304, soft constraints are defined over different objectives:

Progress over a longitudinal goal x_g, Θ _x, = |x - x_g |; over a target speed v_g, Θ _v = |v - v_g |;and over lane tracking, Θ _y = |y|.
Comfort as a minimization of the norm of lateral acceleration, Θ _a , = |a_y |.
Calculated Risk using the big-M formulation, with w ⁺ and l⁺ being the values to the added to the width and length of each Rⁱ based on different risk levels, defined for i ∈ {1,...,n}: $ξ^{i} = \max ((x_{\min}^{i} - l^{+} / 2) - x, 0) + \max (x - (x_{\max}^{i} + l^{+} / 2), 0)$
with the auxiliary constraints: $\begin{array}{r} η^{i} \leq \frac{2 |y - y^{i}|}{w^{+}} \\ \max (1 - η^{i}, 0) - {Mξ}^{i} \leq {\overline{θ}}_{Σ}^{i} \\ \max (1 - η^{i}, 0) + {Mξ}^{i} \geq {\overline{θ}}_{Σ}^{i}, \end{array}$
such that minimizing risk corresponds to minimizing the function $Θ_{Σ} = \sum_{i = 1}^{n} {\overline{θ}}_{Σ}^{i}$
.

4) MILP Problem Definition: With constraints
and cost function
the planning problem of the first stage 302 is formulated as a MILP problem with a receding horizon of K steps.
In the end, a full trajectory x̅ _0:N of length N+1 is obtained at the first stage 302, to initialize the second stage 304. However, the receding horizon approximation iteratively breaks the problem down into a set of simpler optimization problems, each defined over K < N steps. In each iteration a "cost component" of the full cost function is defined as the sum of the individual costs over only K steps, as set out below.
Problem 3 (Receding Horizon MILP). Given an initial ego state x ₀, trajectories of other traffic participants ( $o_{0 : N}^{1 : n}, Σ_{0 : N}^{1 : n}$
), a set of soft constraints
, and planning step (iteration) 0 ≤ m ≤ N - K of the receding horizon, compute the optimal policy: $\begin{array}{l} \begin{matrix} {\overline{u}}_{m : m + K - 1}^{*} = \underset{{\overline{u}}_{m : m + K - 1}}{argmin} \\ s . t \end{matrix} & \begin{array}{l} \sum_{k = m}^{m + K} \sum_{Θ_{ι}, Ω_{ι} \in C} Ω_{ι} Θ_{ι} ({\overline{x}}_{k}, {\overline{u}}_{k}, o_{k}^{1 : n}, Σ_{k}^{1 : n}) \\ \forall k \in \{m, \dots, m + K\} : \\ {\overline{x}}_{k + 1} = F_{Δ t} ({\overline{x}}_{k}, {\overline{u}}_{k}) \\ a_{\min}^{x} \leq a_{k}^{x} \leq a_{\max}^{x} \\ a_{\min}^{y} \leq a_{k}^{y} \leq a_{\max}^{y} \\ |a_{k + 1}^{x} - a_{k}^{x}| < Δ a_{\max}^{x} Δ t \\ |a_{k + 1}^{y} - a_{k}^{y}| < Δ a_{\max}^{y} Δ t \\ v_{\min}^{x} \leq v_{k}^{x} \leq v_{\max}^{x} \\ v_{\min}^{y} \leq v_{k}^{y} \leq v_{\max}^{y} \\ v^{x} \leq ρ |v^{y}| \\ d + b_{l}^{M} (x_{k}) \leq y_{k} \\ yk \leq b_{r}^{M} (x_{k}) - d \\ y_{k, \max}^{i} - {Mμ}_{k}^{i} \leq yk, i \in \{1, \dots, n\} \end{array} \end{array}$
For planning step m = 0, the known initial vehicle state x ₀ is used as a starting point, with subsequent states determined by applying the vehicle dynamics model F _Δt in an iterative fashion. For each subsequent planning step, the state x _m-1 determined in the previous planning step m - 1 provides a starting point for that iteration, with subsequent states computed from that starting point in the same way.
In both the MILP and the NLP stages, optimization occurs in the continuous space; no spatial/grid discretization occurs at either of the stages. In particular, in the MILP stage, the state vectors of both the ego vehicles and the other actors, and control vectors are not restricted to integer values, or otherwise discretised to any fixed grid. Not discretizing over a grid is one of the advantages of the method, as it allows for smoother trajectories.
The integer variables in the MILP stage are used to enforce the constraints related to collision avoidance and driveable surface only.
The trajectory x _0:N computed in the MILP optimization stage 302 is an example is a "seed trajectory", as that term is used herein, whereas the trajectory x _0:N computed in the NLP optimization stage 304 (derived, in part, from the seed trajectory x _0:N), is an example of a "final" trajectory. The trajectory is "final" within the specific context of the two-stage optimization. The term "final" does not necessarily imply finality within the context of the runtime 100 stack as a whole. The final trajectory x _0:N may or may not be subject to further processing, modification etc. in other parts of the runtime stack 100 before it is used as a basis for generating control signals within the controller 108.
Consistent with the above terminology, the linear cost function of Equation (22) and the linear dynamics model of Equations (15) and (16) are examples of a "preliminary" cost function and model respectively, whilst the non-linear cost function of Equation (11) and the non-linear dynamics model of Equation (5) are examples a "final" or, equivalently, "full" cost function and model respectively, where the terms "final" and "full" are, again, only used in the specific context of the two-stage optimization, and does not imply absolute finality.
It is noted that, which the above paragraph considers seed and final trajectories, it could be that the control actions u _0:N-1 of the MILP stage 302 are alternatively or additionally used to seed the NLP stage 304 (and they may be referred to as seed control actions in that event); similarly, as noted above, data of one or both of the final trajectory x _0:N and the final series of control actions u _0:N-1 may be provided to the controller 108 for the purpose of generating "actual" control signals for controlling the autonomous vehicle.
Whereas in the NLP stage 304, the scenario description parameters (x ₀, $O_{0 : n}^{1 : N}, Σ_{0 : n}^{1 : N}$
) have a non-linear form (because they are formulated in terms of heading φ_k ), in the MILP stage 302, the scenario description parameters (x ₀, $O_{0 : n}^{1 : N}, Σ_{0 : n}^{1 : N}$
) have a linear form (because motion is formulated in terms of components $v_{k}^{x}, v_{k}^{y}$
instead of speed and heading v_k ,φ_k ), with x ₀ denoting the linearized form of the ego vehicle state specifically.
Regarding notation, x _0:N and x _0:N are used as shorthand to represent complete trajectories, including the (known) current state x ₀. The additional states actually determined in the relevant optimization stage 302, 304 are x _1:N and x _1:N, as depicted in Figure 3.
Figure 4 shows, by way of example, one example of a possible trajectory over eight time steps (k=0 to 7). This corresponds to an overtaking maneuver, in which an ego vehicle 400 desired to overtake a forward vehicle 402 in the presence of an oncoming vehicle 404. Both the forward vehicle and the on-coming vehicle are traffic participants (obstacles), modelled in the manner above.
It will be appreciated that the duration Δt between time steps in Figure 4 has been chosen for the purpose of illustration, in order to show the whole manoeuvre, and would not necessarily reflect the duration that would be applied in real-life.
An overtaking goal is defined by way of a suitably distant reference location (not shown) on the reference path
, ahead of the forward vehicle 402. The effect of the progress constraints is to encourage the ego vehicle 400 to reach that reference location as soon as it can, subject to the other constraints, whilst the effect of the collision avoidance constraints is to prevent the ego vehicle 402 from pulling out until the oncoming vehicle stops being a collision risk.
Figure 5 shows how such a problem might be formulated in the two-stage approach. Shading is used to show the ego vehicle 400 and the forward vehicle 402 at different timesteps k = 0, ... ,7 (For the sake of simplicity, the scenario of Figure 5 is slightly different than of Figure 4, in that it omits the oncoming vehicle 404).
On the left-hand side, Figure 5 illustrates some of the principles underlying the MILP formulation of the first stage 302.
As can be seen on the left-hand side of Figure 5, the ego vehicle 400 remains axially aligned (i.e. aligned with the reference path
) at all times. This reflects the simpler, linear dynamics model that is used in the MILP stage - as can be seen from Equations (15) and (16) above, the simpler dynamics model does not consider rotation of the ego vehicle 200 (recall that the MILP stage uses a linear representation of velocity and acceleration). Hence, the output of the first stage 302 is a more "restricted" form of trajectory defined in terms of linear position and velocity but not orientation/heading of the ego vehicle 200. The linearized trajectory may not be fully dynamically realisable, but nevertheless serves as a useful initialization to the second stage 304.
By contrast, as shown on the right-hand side of Figure 5, the form of the trajectory planned in the second stage 304 is not restricted in the same way the first stage 302. This is because the full problem takes into account steering angle and heading of the ego vehicle 200. That is, the solution to Problem 2 is free to consider motion of the ego vehicle 400 that does not meet the discretisation restrictions applied in the first stage 302.
5) On the optimality of the MILP formulation: The goal of the formulation of Problem 3 is to obtain an optimal solution which can be used as initialization to Problem 2 to minimize the problems of slow convergence and local optima that arise from solving the NLP directly. In this section, the optimality of the solution to Problem 3 is discussed.
A solution to Problem 3 can be obtained using Branch and Bound, a divide and conquer algorithm first introduced and applied to mixed integer linear programming by Land and Doig in [10]. This solution is proven to be the globally optimal one [7], [11]. In practice, modern solvers (e.g. Gurobi or CPLEX) may fail to find the global solution due to rounding errors and built-in tolerances [11]. On top of this, the receding horizon formulation of the problem, introduced for the sake of computational tractability, generates suboptimality by definition [12], [13]. Due to these factors, no strong theoretical guarantee can be given regarding the solution of the MILP stage. However, despite the lack of theoretical guarantees at this level, a solution that is close to the global optimum at each receding horizon step acts as a proxy towards a final solution to be obtained in the second stage 304 that is close to the global optimum and, in turn, initializing the NLP stage using this solution is expected to improve the quality of the solution at that second stage 304.
In some contexts, it may be that the speed increase provided by the two-stage approach will admit direct application on an autonomous vehicle in real-time.
However, to provide an additional speed increase, which may be needed to ensure real-time performance in a resource-constrained environment, it is also possible to implement an approximation of one or both of the first and second stages 302, 304, using a trained function approximator, such as a neural network.
Either one or both of the first and second stages 302, 304 could be replaced with a function approximator in a practical implementation.
By way of example, the following section describes, with reference to Figure 6, an example of how the non-linear second stage 304 might be with a trained function approximator.
Figure 6 shows how the two-stage optimization can be run repeatedly, in an offline context (e.g. in an off-board training computer system), for different scenarios, for the purpose of generating an NLP training set 600. In this case, each input in the NLP training set 600 corresponds to an initialization output generated by the first stage optimizer 302 on a given scenario, and a ground truth label for that inputs is given by the output of the second stage optimization 304 on that training scenario. Within this framework, the NLP optimization stage 304 is treated as a function that takes the initialization output of the first stage 302, and transforms it to a desired control output (corresponding the optimal trajectory solving Problem 2). With a sufficiently large NLP training set 600, a function approximator 304 can be trained to approximate this function of the NLP optimization stage 304. The trained function approximator 604 (which may be referred to as the NLP approximator 602) can, in turn, be implemented on-board the vehicle 400, rather than attempting to perform the full non-linear optimization of the second stage 304 on-board the vehicle in real-time.
Each training example 601 of the NLP training set is made up of an input, which in turn comprises a particular set of scenario description parameters (x₀, $O_{0 : n}^{1 : N}, Σ_{0 : n}^{1 : N}$
) together with a corresponding seed trajectory x _0:N (i.e. as computed in the MILP stage 302 for the corresponding scenario description parameters), and a corresponding ground truth, which in this case is a corresponding final trajectory x _0:N as computed in the NLP stage 304 for the corresponding input.
Once trained, given such an input, i.e. a set of scenario description parameters (x ₀, $O_{0 : n}^{1 : N}, Σ_{0 : n}^{1 : N}$
) and a seed trajectory x _0:N (as computed in an online instantiation of the MILP phase 302 in this example), the NLP approximator 900 computes a corresponding final trajectory x̃ _0:N which will approximate a full NLP solution x _0:N that would be obtained from the full NLP optimization ( x _0:N may be referred to as the exact final trajectory in this context). Because the NLP stage 304 is now approximated, it will not necessarily be guaranteed that the approximate final trajectory x̃ _0:N will meet the hard NLP constraints of Problem 2 above (i.e. the hard constraints on which the NLP approximator 604 has been trained). Therefore, a trajectory verification component 702 is provided, whose function is to verify the approximate final trajectory x̃ _0:N against the hard NLP constraints (i.e. the constraints as set out above in relation to Problem 2), or some practical variant of the hard NLP constraints (e.g. the NLP approximator 712 could be trained "conservatively" on somewhat more stringent hard constraints than the constraints the approximate final trajectory x̃ _0:N is actually required to satisfy, in order to reduce instances in which the approximate final trajectory x̃ _0:N fails the actual hard constraints imposed by the trajectory verification component 712). That is to say, the trajectory verification component 712 verifies the approximate final trajectory x̃ _0:N against a set of trajectory verification constraints, which may or may not be identical to the NLP constraints of Problem 2.
If the approximate final trajectory x̃ _0:N satisfies the trajectory verification constraints, it can be passed to the controller 108 to implement. In the case that the approximate final trajectory fails to satisfy at least one of the trajectory verification constraints, then it can modified, either at the planning level (by the planner 106) or control level (by the controller 108) so that it does. Assuming the NLP approximator 702 has been adequately trained such modifications, if required, should generally be relatively small.
The different scenarios could, for example, be simulated scenarios, generated in a simulator.
Figure 7 shows how a similar approach may be adopted to approximate the MILP stage 302 online. The same principles apply, however in this case, a MILP training set 700 is created through repeated applications of the MILP optimizer on different scenarios, and each training example 701 of the MILP training set 700 is made up of an input in the form of particular set of scenario description parameters (x ₀, $O_{0 : n}^{1 : N}, Σ_{0 : n}^{1 : N}$
) (mirroring the input to the MILP stage 302) and a corresponding ground truth, which is now the corresponding seed trajectory x _0:N (i.e. as obtained for the corresponding input parameters in the MILP stage 302).
That is to say, whereas the seed trajectory x̅_0:N forms part of the input of each training example 601 of the NLP training set 600 of Figure 6 (with the full trajectory x ₀ providing the ground truth), in the MILP training set 700 of Figure 7, the seed trajectory x _0:N now provides the ground truth.
The same principles generally apply, but now a function approximator 702 is trained on the MILP training set 700 to approximate the MILP stage 602. Such a trained MILP approximator 702 can then be used in online processing to compute an approximate seed trajectory ${\tilde{\overline{x}}}_{0 : N}$
given a set of online scenario description parameters (x₀, $O_{0 : n}^{1 : N}, Σ_{0 : n}^{1 : N}$
), and the approximate seed trajectory ${\tilde{\overline{x}}}_{0 : N}$
can, in turn, be used to initialize an online instantiation of the NLP stage 304. In this example, the full NLP stage 304 is implemented, and the trajectory verification component 712 is therefore not required.
Regarding notation, x̃ _0:N and ${\tilde{\overline{x}}}_{0 : N}$
are used as shorthand to represent complete trajectories, including the current state x₀. The states actually determined by the relevant function approximator are x̃ _0:N and ${\tilde{\overline{x}}}_{1 : N}$
. Certain figures show x̃ _0:N and ${\tilde{\overline{x}}}_{0 : N}$
as an output of a function approximator. It will be appreciated that this is a shorthand decision of the computed states x̃ _1:N / ${\tilde{\overline{x}}}_{1 : N}$
(as applicable), together with the known initial state x ₀ provided as an input to the system. This also applies to Figures 8 and 9 (see below).
Figure 8 shows an example of an implementation in which the MILP stage 302 and the NLP stage 304 are both approximated. In this case, the MILP approximator 702 of Figure 7 is used to seed the NLP approximator 604 of Figure 6, i.e. an approximate seed trajectory ${\tilde{\overline{x}}}_{0 : N}$
forms an input to the NLP function approximator 604, which in turn is used in deriving an approximate full trajectory x̃ _0:N . The trajectory verification component 712 is used, as in Figure 6, to verify the approximate full trajectory x̃ _0:N.
Figure 9 shows another example implementation, in which a single function approximator 900 is trained to approximate both optimization stages 302, 304. In this case, the function approximator 900 takes, as input, a set of scenario description parameters, and directly computes an approximate full trajectory x̃ _0:N. The single approximator 900 would be trained on training examples whose inputs are scenario description parameters and whose ground truths are provided by the corresponding full NLP trajectories. A trajectory verification component 732 is similarly used in this case to verify the output of the single function approximator 900.
Note that the term "approximate" is used herein in two somewhat distinct senses. When a function approximator is used to approximate the MILP stage 302 and/or the NLP stage 304, the MILP / NLP function approximator 302 / 304 is said to approximate the seed trajectory x _0:N / final trajectory x _0:N as applicable (i.e. the output of the trained MILP approximator 702, ${\tilde{\overline{x}}}_{0 : N}$
, is said to be an approximation of the exact seed trajectory x _0:N that would be derivable via the full MILP optimization; likewise, the output of the trained NLP approximator 704, ~x _0:N, is said to be an approximation of the exact final trajectory x _0:N that would be obtained via the full NLP optimization).
The seed trajectory ${\overline{x}}_{0 : N} / {\tilde{\overline{x}}}_{0 : N}$
(whether exact or approximate in the sense of the preceding paragraph) is also said to approximate the full trajectory x _0:N / x̃ _0:N (whether exact or approximate in the sense of the preceding paragraph). The exact or approximate seed trajectory ${\overline{x}}_{0 : N} / {\tilde{\overline{x}}}_{0 : N}$
may be a relatively coarse approximation of the exact or approximate final trajectory $x_{0 : N} / {\tilde{x}}_{0 : N}$
. A relatively coarse approximation in this context is perfectly acceptable, because even a coarse seed trajectory (i.e. x _0:N or ${\tilde{\overline{x}}}_{0 : N}$
) can have the desired effect of guiding the NLP optimization toward globally a optimal solution (and away from mere local optima). In this same sense, the linear cost function of Equation (22) approximates the non-linear cost function of Equation (11) and the linear dynamics model of Equations (15) and (16) approximates the non-linear dynamics model of Equation (5).
Note that, whilst the description of Figures 6 to 9 assumes that the various components operates on trajectories x _0:N or ${\tilde{\overline{x}}}_{0 : N}$
, they could, alternatively or additionally, operate on the control actions computed at the various optimization/approximation stages, i.e. each approximator 602, 704 could alternatively or additionally be trained to determine an approximate series of seed control actions ${\tilde{\overline{u}}}_{0 : N - 1}$
(approximating those of the MILP stage 302) and/or an approximate series of final control actions ũ _0:N-1 (approximating those of the NLP stage 304 - and which could in turn form (part of) or be used to derive the input to controller 108). The same applies to the single approximator of Figure 9, which could alternatively or additionally compute ũ _0:N-1 directly from the scenario description parameters.
The above techniques be implemented in an "onboard" or "offboard" context. One example of an offboard context would be the above training performed in an offboard computer system. The above techniques can also be implemented as part of a simulated runtime stack in order to test the performance of the runtime stack in a simulator. Simulation is an increasingly crucial component of safety and other performance testing for autonomous vehicles in particular.
Note, where a function approximation approach is used, the function approximator(s) can be tested in the simulator, in order to assess their safety and performance before they are deployed on an actual vehicle.
Nonlinear non-convex constrained optimization methods are not guaranteed to converge even if feasible solutions exist. They may, therefore, be deployed with the implementation of a fallback mechanism (e.g. emergency braking), which is not necessarily guaranteed to meet the same safety constraints. A goal in this context is to improve the convergence and solution quality of existing nonlinear constrained optimization methods, thus improving the safety of the overall system by reducing the reliance on the potentially unsafe fallback mechanism, while yielding lower-cost plans. This is achieved via an informed initialization step.
As noted, the present techniques are not limited to real-time applications. The described techniques for improving convergence and achieving higher quality solutions might compromise runtime efficiency, depending on the implementation, but such implementations may nevertheless be suitable for applications that can tolerate longer planning times in return for higher-quality plans - such as high-fidelity simulation or urban driving.
References herein to components, functions, modules and the like, including the optimizers of the first and second stages 302, 304, the function approximators 702, 604 and 900, and the verification component 712, denote functional components of a computer system which may be implemented at the hardware level in various ways. A computer system comprises one or more computers that may be programmable or non-programmable. A computer comprises one or more processors which carry out the functionality of the aforementioned functional components. A processor can take the form of a general-purpose processor such as a CPU (Central Processing unit) or accelerator (e.g. GPU) etc. or more specialized form of hardware processor such as an FPGA (Field Programmable Gate Array) or ASIC (Application-Specific Integrated Circuit). That is, a processor may be programmable (e.g. an instruction-based general-purpose processor, FPGA etc.) or non-programmable (e.g. an ASIC). A computer system may be implemented "onboard" a mobile robot (such as an AV) or "offboard" for example in a simulator context.
For example, an offboard implementation of the full multi-stage optimization could be implemented for the purpose of training on-board components, but also for other purposes, such as safety testing or other performance testing, verification etc. Performance testing could involve implementing any of the above trained function approximators 702, 604, 900 and (where applicable) the verification component 712 in an offboard computer system as part of the performance testing, applied to simulated scenarios.
In an on-board implementation, the multi-phase optimization may be implemented in full, or one or more (including all) optimization stages may be replaced with one or more function approximators.

References

Reference is made in the above to the following:

[1] W. Schwarting, J. Alonso-Mora, L. Paull, S. Karaman, and D. Rus, "Safe nonlinear trajectory generation for parallel autonomy with a dynamic vehicle model," IEEE Transactions on Intelligent Transportation Systems, vol. 19, no. 99, 2017.
[7] I.E. Grossmann, V. Voudouris, O. Ghattas, et al., "Mixed-integer linear programming reformuilations [sic] for some nonlinear discrete design optimization problems," Technical Report, Carnegie Mellon University, 1991.
[8] H.P. Williams, Model building in mathematical programming. John Wiley & Sons, 2018
[9] J. Omer and J.-L. Farges, "Hybridization of nonlinear and mixed-integer linear programming for aircraft separation with trajectory recovery," IEEE Transactions on Intelligent Transportation Systems, vol 14, no. 3, pp. 1218-1230, 2013.
[10] A. H. Land and A. G. Doig, "An automatic method for solving discrete programming problems," in 50 Years of Integer Programming 1958-2008, pp. 105-132, Springer, 2010.
[11] A. Neumaier, "Complete search in continuous global optimization and constraint satisfaction," Acta numerica, vol. 13, pp. 271-369, 2004.
[12] X. Geng and Y. Xi, "Suboptimality analysis of receding horizon predictive control with terminal constraints," IFAC Proceedings Volumes, vol. 32, no. 2, pp. 2984-2988, 1990.
[13] L. Grune and A. Rantzer, "On the infinite horizon performance of receding horizon controllers," IEEE Transactions on Automatic Control, vol. 53, no. 9, pp. 2100-2111, 2008.

Annex A - Results and evaluation for 2s-OPT planner

The methodology described above has been subject to empirical testing to further demonstrate is efficacy.
Reference is made to Figure 10, which shows, in the top half, an overtaking scenario somewhat analogous to that of Figures 4 and 5. Specifically, the top scenario of Figure 10 shows a scenario with overtaking of static vehicles 402 with an oncoming vehicle 404 in residential driving.
The bottom half of Figure 10 shows an alternative, junction scenario, which uses like reference numerals to denote the ego vehicle 400 and an external actor 402. This is an unprotected right turn in a junction.
In this section we will show that:

1) our method is general and can be applied to a diversity of driving situations
2) the MILP stage provides the NLP stage with a better initialization when compared to simpler heuristics, leading to higher convergence rates, faster solving times and more optimal solutions;
3) our method leads to solutions that outperform a Nonlinear Model Predictive Control (NMPC) approach similar to the one presented in [E-25] in progress and comfort metrics.

To that end, we implement the first stage (MILP; Problem 3) using Gurobi 8.1 [E-15], and the second stage (NLP; Problem 2) using IPOPT [29]. Both solvers have a timeout of 25s, after which the optimization is stopped. We use N = 40 and Δt = 0.2s for a trajectory horizon of 8s (Other parameters are listed in Appendix A). Without loss of generality, we assume left-hand traffic where drivers are expected to be on the left hand side of the road, that a route planner is available to generate a reference path the satisfies the planning goal in the local coordinate frame, and consider a constant velocity model for the prediction of dynamic agents. In the simulator, the behavior of other dynamic vehicles is based on the Intelligent Driver Model [E-27].
VI.1 Generality: Figures 11 and 12 show two qualitatively distinct driving situations. In Figure 11 the ego-vehicle overtakes parked vehicles on a two-lane road while taking into account an oncoming vehicle on the other lands that is also overtaking a static vehicle in its own lane (the reference path) for overtaking, and returning to it as soon as possible, while always maintaining a smooth acceleration profile. The second example in Figure 12 shows an unprotected right turn into a main road. Figure 12(d) shows the ego-vehicle closely reference path (right turn into the main road) while smoothly responding to unforeseen behaviors of the dynamic vehicles.
Figure 13 shows examples of the four different simulation scenarios we consider for experiments 2) and 3): (SO) static overtaking of parked vehicles on both side of the road; (SO+OV) static overtaking with a dynamic vehicle in the oncoming lane; (DO) dynamic overtaking of a slow moving vehicle; and (DO+OV) dynamic overtaking with an oncoming vehicle on the other lane. For the experiments, we generate datasets of examples per scenario by randomizing over relevant parameters (e.g. initial speeds and numbers of vehicles). More details are presented in Supplementary Material - Appendix B.
VI. 2 Initialization Comparison: We consider four heuristic initializations as alternatives to our MILP stage: ZEROS, in which all states and controls are initialized to zero; C.VEL, where the ego-vehicle is assumed to maintain its speed throughout the solution; C.ACC, where the ego-vehicle maintains a constant acceleration of 1 ms^-2 until the maximum speed is achieved; and C.DEC where the ego-vehicle maintains a constant acceleration of -1 ms^-2 until it stops.
A dataset of 1000 examples per scenario was procedurally generated, solved using our method as well as suing only the NLP stage for the same constrains initialized by each of the four heuristics. Table D-I presents the percentage of solved examples within the allocated time (25s) per initialization method and scenario. As it can be observed, our method either performs similarly or outperforms other methods in all the scenarios considered. In Figure 14 we analyze the problems that were solved using our initialization when compared to the other initialization methods. We observe that, across scenarios ZEROS and C.DEC lead to the highest percentage of solved problems (after ours) and same solutions, but also the highest percentage of different solutions. To analyze this further, for problems where a different solution is reached (darker bars in Figure 14) Table II shows the average % difference in cost that the alternatives achieve compared to ours. For problems with similar solutions (lighter bars in Figure 14), Table II presents the average % difference in solving time.
We see that, in general, other initialization methods obtain higher costs on average than ours. While C.ACC is able to achieve a lower average cost in some scenarios, it leads to a substantial number of unsolved examples, particularly for DO and DO+OV (see Figure 14). In all scenarios where the same solutions is reached by an alternative method, it takes longer to solve on average. A noteworthy case is DO+OV initialized with C.ACC which takes 306.32% longer on average to reach the same solution as ours. These results validate our claim that we achieve higher convergence rates (as shown by Table I), faster solving times and more optimal solutions (as evidenced by Figure 14 and Table II).
VI.3 NMPC Baseline Comparison: We compare our method to an NMPC baseline which optimizes Problem 2 with the same soft and hard constraint but in a receding-horizon fashion similar to [25]. This baseline uses the same interior-point optimization method as our NLP stage to solve each of the horizon steps. To evaluate the quality of the methods, we introduce the following metrics that compare our solution, $(x_{1 : N}^{Ours}, u_{1 : N - 1}^{Ours})$
, to the baseline's, $(x_{1 : N}^{NMPC}, u_{1 : N - 1}^{NMPC})$
:

Progress: our overall progress compared to the baseline's $Δ P = \max (x_{1 : N}^{Ours}) - \max (x_{1 : N}^{NMPC})$
Reference path deviation: our average deviation from the reference path compared to the baseline's $Δ D = \frac{1}{N - 1} \sum_{k = 1}^{N} |y_{k}^{NMPC}| - |y_{k}^{Ours}|$
Target speed matching: for a given target speed v_g, comparison of our average matching with the baseline's $Δ v_{M} = \frac{1}{N - 1} \sum_{k = 1}^{N} |v_{k}^{NMPC} - v_{g}| - |v_{k}^{Ours} - v_{g}|$
Jerk: comparison of our average jerk value with the baseline's $Δ |a| \frac{1}{N - 1} \sum_{k = 1}^{N} \frac{a_{k}^{NMPC} - a_{k - 1}^{NMPC}}{Δ t} - \frac{a_{k}^{Ours} - a_{k - 1}^{Ours}}{Δ t}$

We generate 1000 examples per scenario and solve them using our method and the NMPC baseline. The top of Table III presents the percentage of the examples that are solved by both methods. In all scenarios, our method solves more examples than the baseline. For the problems that are solved by both we show the metrics per scenario in the bottom of Table III. We achieve significantly higher progress and better velocity matching across scenarios, and similar or slightly smaller jerk values and deviation from the reference path. These results validate our claim that our method is better in progress and comfort metrics than the baseline that has a similar formulation to [E-25].

TABLE I: Initialization Comparison: percentage of solved examples per initialization method (rows) and scenario (columns).

	SO	SO+OV	DO	DO+OV
ZEROS	98.57%	95.55%	96.97%	93.48%
C.VEL	96.85%	85.68%	42.42%	32.40%
C.ACC	89.23%	64.43%	5.61%	1.97%
C.DEC	98.58%	95.66%	88.79%	92.44%
Ours	98.47%	95.77%	99.40%	98.14%

TABLE III: NMPC Baseline Comparison: Top - percentage of solved problems per scenario. Bottom - average results for difference in progress ΔP, deviation from reference ΔD, velocity matching Δu_M, and jerk Δ|ȧ| (positive and higher is better).

		SO	SO+OV	DO	DO+OV
% Solved	NMPC	96.15%	79.45%	89.17%	85.74%
% Solved	Ours	98.38%	96.50%	99.60%	98.65%
Metrics	ΔP (m)	7.12	8.37	4.63	8.04
	ΔD (m)	-0.03	0.06	0.10	0.03
	$Δ υ_{M} (\frac{m}{s})$	0.81	0.79	0.25	0.49
	$Δ \|\dot{a}\| (\frac{m}{s^{3}})$	0.01	0.01	0.01	0.0

TABLE II: Initialization Comparison: in cases where the optimum is different (left), the average percent difference in the objective function value (cost) between the alternative method and our solution is show (positive and higher is better for our method), with the respective number of examples in parenthesis; in cases in which the optimum is the same (right), the average difference in solving time as a percent of the one of our method is shown (positive and higher is better for our method), with the number of examples in parenthesis.

Init.	% Change in Cost (# situations)				% Change in Solving Time (# situations)
Init.	SO	SO+OV	DO	DO+OV	SO	SO+OV	DO	DO+OV
ZEROS	+85.39(56)	+68.53(126)	+52.36(356)	+25.96(403)	+42.8(902)	+47.62(728)	+154.42(607)	+104.44(483)
C.VEL	+60.24(44)	+47.69(112)	+29.55(64)	+1.35(126)	+12.53(903)	+16.39(652)	+70.01(358)	+94.71(187)
C.ACC	+6.16(20)	-10.77(100)	+0.01(2)	-7.67(5)	+12.93(856)	+19.16(478)	+231.79(54)	+306.32(14)
C.DEC	+85.01(55)	+67.39(123)	+36.79(322)	+24.51(396)	+ 7.93(90:3)	+3.48(782)	+70.2:3(5G5)	+ 17.61(483)

A. Optimization parameters

The parameters used in the solving of

Problem

2 and 3 in the context of Sec. VI are defined in Table A1

TABLE A1: Parameters used in the MILP and NLP optimization stages

Parameter	Stage	Value	Parameter	Stage	Value
L	NLP	4.8	$a_{\min}^{x}$	MILP	-3
δ _max	NLP	0.45	$a_{\max}^{x}$	MILP	3
α _min	NLP	-3	$a_{\min}^{y}$	MILP	-0.5
α _max	NLP	3	$a_{\max}^{y}$	MILP	0.5
ȧ _max	NLP	0.5	$Δ a_{\max}^{x}$	MILP	0.5
δ̇ _max	NLP	0.18	$Δ a_{\max}^{y}$	MILP	0.1
ȧ _max	NLP	0.5	$υ_{\min}^{x}$	MILP	0
v _min	NLP	0	$υ_{\max}^{x}$	MILP	3
v _max	NLP	10	$υ_{\min}^{y}$	MILP	-1
ω_x	NLP	0.1	$υ_{\max}^{y}$	MILP		1
ω_v	NLP	2.5	Ω _x	MILP	0.9
ω_y	NLP	0.05	Ω _v	MILP	0.5
ω_a	NLP	1.0	Ω _y	MILP	0.05
ω_δ	NLP	2.0	Ω _a	MILP	0.4
ρ	MILP	1.5	M	MILP	10⁴
d	MILP	0.9

B. Generalization of scenario examples

For the randomly generated scenarios presented in Sec. V, we assume the ego vehicle has length 4.8m and width 1.9m. the example used in this work were preceduirally generated by uniform sampling the parameters of the scenarios, following the ranges defined in Tables B1, B2, B3, B4, and B5.

TABLE B1: Common parameters

Parameter		Min	Max
Number of lanes		2	2
Lane width (m)		3.5	4.3
Ego	initial x (m)	0	0
	initial y (m)	b_l (x) + 0.55 * 1.9	b_r (x) - 0.55 * 1.9
	initial v (ms ^-1)	0	9.5
	initial φ (rad)	-π/12	+π/12

TABLE B2: Parameters of scenario so

Parameter		Min	Max
Number of static vehicles		2	6
Static vehicle	x (m)	0	80
	y (m)	b_l (x)	b_r (x)
	width (m)	1.7	2.5
	length (m)	4.0	8.0

TABLE B3: Parameters of scenario SO+OV

Parameter		Min	Max
Number of static vehicles		2	6
Static vehicle	x (m)	0	80
	y (m)	b_l (x)	0
	width (m)	1.7	2.5
	length (m)	4.0	8.0
Oncoming vehicle	initial x (m)	20	80
	initial y (m)	b_r (x)/2	b_r (x)/2
	initial v (ms ^-1)	1.0	8.5
	width (m)	1.7	2.5
	length (m)	4.0	8.0

TABLE B4: Parameters of scenario DO

Parameter		Min	Max
Dynamic vehicle	initial x (m)	20	80
	initial y (m)	b_l (x)/2	b_l (x)/2
	initial v (ms ^-1)	0.5	3.5
	width (m)	1.7	2.5
	length (m)	4.0	8.0

TABLE B5: Parameters of scenario DO+OV

Parameter		Min	Max
Oncoming vehicle	initial x (m)	20	80
	initial y (m)	b_r (x)/2	b_r (x)/2
	initial v (ms ^-1)	1.0	8.5
	width (m)	1.7	2.5
	length (m)	4.0	8.0
Dynamic vehicle	initial x (m)	20	80
	initial y (m)	b_l (x)/2	b_l (x)/2
	initial v (ms ^-1)	0.5	3.5
	width (m)	1.7	2.5
	length (m)	4.0	8.0

Claims

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 (302) to determine a first series of control actions that substantially globally optimizes a preliminary cost function for the scenario, wherein the desired goal 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 (304) to determine a second series of control actions that substantially globally optimizes a full cost function for the scenario, wherein the desired goal is fixed during the second constrained optimisation 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 (304) 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 (304) to the substantially globally-optimal second series of control actions.
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 the 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.
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.
The method of any preceding claim, wherein the first optimizer (302) is a mixed integer linear programming (MILP) optimizer, and the second optimizer (304) is a non-linear programming (NLP) optimizer;
wherein the first optimizer (302) optionally 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 (304) optionally does not use any receding horizon approximation and instead optimizes the full cost function as a whole.
The method of claim 4, wherein the hard constraints of the first stage (302) 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 (304) comprise one or more similar collision avoidance and/or permitted area constraints formulated in terms of non-integer variables.
The method of any preceding claim, 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;
wherein each of the computed trajectories is optionally represented in a frame of reference defined by the reference path;

wherein the preliminary cost function is optionally linearly dependent on said lateral and/or longitudinal deviation, and the full cost function is optionally non-linearly dependent thereon;

wherein both cost functions optionally penalize deviation from a target speed.
The method of any preceding claim, wherein the method is implemented in a planner (106) 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.
The method of any of claim 1 to 6, wherein the method is performed repeatedly for different scenarios so as to generate a first training set (700) comprising inputs to the first optimizer (302) and corresponding outputs computed by the first optimizer (302), and the training set is used to train a first function approximator to approximate the first optimizer (302) and;
wherein the method is optionally performed repeatedly for different scenarios so as to generate a second training set (700) comprising inputs to the second optimizer (304) and corresponding outputs computed by the second optimizer (304), and the training set is used to train a second function approximator to approximate the second optimizer (304).
The method of any of claims 1 to 6, wherein the method is performed repeatedly for different scenarios so as to generate a training set comprising inputs to the first optimizer (302) and corresponding outputs computed by the second optimizer (304), and the training set is used to train a single function approximator to approximate both of the first (302) and the second optimizers (304), the method optionally comprising the step of configuring a runtime stack (100) of mobile robot to implement the single function approximator.
The method of claim 8, comprising the step of configuring a runtime stack (100) of mobile robot to implement one of the following combinations:
(i) the first optimiser (302) and the second function approximator, wherein the second function approximator cooperates with the first optimiser (302) within the run-time stack (100) to approximate the second optimization stage (304);

(ii) the first function approximator and the second optimizer (304), wherein the first function approximator approximates the first optimization stage (302) for initializing the second optimizer (304); and

(iii) the first function approximator and the second function approximator, which cooperate to approximate both optimization stages.
The method of claim 10, wherein the runtime stack (100) 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 (100) with a verification component (702) configured to verify an output of the second function approximator or the single function approximator.
The method of any of claims 8 to 11, wherein the or each function approximator has a neural network architecture.
A computer system comprising
one or more optimization components configured to implement the methods of claims 1 to 12.
The computer system of claim 13 , embodied in a mobile robot, wherein the computer system is further configured to control the motion of the mobile robot via one or more actuators (112) of the mobile robot using control data provided by the second optimization component (304).
The computer system of claim 14, wherein the mobile robot is an autonomous vehicle.
A computer program for programming one or more computers to implement the method of any of claims 1 to 12.