VEHICLE ROUTES IN CONHNED AREAS

TECHNICAL HELD

[0001] The present disclosure relates to the field of automatic vehicle control. In particular, it proposes techniques for coordinating the movement of multiple vehicles in an environment with conflict zones.

BACKGROUND

[0002] It is believed that fully automated vehicles (AV) have the potential to drastically change the transport industry, both in terms of increased safety and efficiency [1]. The most drastic improvements are expected when a substantial part of the vehicles on public roads are fully automated, as in e.g., ”robo-taxis” and hub-to- hub transports on highways. Unfortunately, managing the unpredictable conditions on public roads in a reliably safe manner has proven to be harder than initially expected, and the current state-of-the-art exhibits a lack of production-level maturity.

[0003] However, confined areas, such as mines, ports and logistic centers, lack many of the difficult aspects of public road driving, and present use cases for near- future, large-scale deployment of automated vehicles. Within this context, the AVs form a component in transport solutions for commercial operations, where the material-flow is largely handled without human involvement.

[0004] One of the challenges in such systems is the efficient coordination of multiple AVs use of mutually exclusive resources, such as intersections, narrow roads, work-stations (e.g. crushers, loading/unloading spots etc.) and, in the case of electrified AVs, charging-stations. Poor coordination can lead to substantial decreases in energy-efficiency and productivity, reducing the benefits of automation. In detail, failure to coordinate the traffic situation efficiently has the following implications:

• Energy waste: With poor planning, the vehicles expend more energy than required to perform the transport missions. An efficient planner, on the other hand, prevents vehicles from coming to unnecessary stops at, e.g., intersections, by pre-emptive speed adjustments. Moreover, an efficient planner harvests potential energy better by, e.g., prioritizing vehicles going downhill in the MUTEX management, avoiding unnecessary deceleration. Energy efficiency is particularly relevant for Battery-Electric Vehicles (BEV), where poor performance leads to more frequent charging stops where no value is added to the transport solution.

• Underutilization: With poor planning, vehicles are often stuck in states where no value is added. Examples are vehicles waiting to enter charging stations or loading/unloading areas. An efficient planner avoids this by performing MUTEX management and speed planning in such a way that waiting times are reduced, leading to more value increasing time spent in the transport solution and a smaller vehicle fleet.

• Deadlocks: Some road network traffic layouts are prone to deadlocks, a situation where the vehicles cannot continue their missions without manual intervention in the form of reset. With a deadlock prone traffic layout, such situations will inevitably occur unless a traffic planner restricts access to certain areas at key instants.

• Safety: With poor planning, it is possible for the system to be near dangerous states (e.g., risk of collision or similar) such that lower-level emergency systems are activated by small disturbances. An efficient planner avoids this by including sufficient margins.

• Physical failure states: With poor planning, vehicles may enter states from which they cannot recover. An example is the complete stop of a heavily loaded vehicle in a steep incline. If stopped, the engine/motor and transmission in use can be such that it cannot be re-started. An efficient planner avoids this by pre-emptive speed planning.

• Hardware wear: An efficient planner avoids unnecessary wear of the machinery, which for instance could be induced when heavy vehicles are decelerated with the service breaks repeatedly.

[0005] The problem of handling mutual exclusive resources has been addressed for industrial robots [16], [17], where different scheduling algorithms have been explored. The coordination of multiple AVs, however, adds a different aspect to the challenge, where for example, the dynamics of the vehicles and the road topology play a significant part in the optimization problem. The coordination of automated vehicles at intersections has been widely discussed in the literature recently, see [2] for a comprehensive survey. In general, the problem is difficult to solve, and was formally shown to be NP-hard in [15]. Often relying on simplifying assumptions and heuristics, a number of methods has been presented that solve the problem using, e.g., hybrid system theory [3], reinforcement learning [4], scheduling [5], model predictive control (MPC) [6], [7] or direct optimal control (DOC) [8], [9].

[0006] Coordination of AVs in confined areas has some distinct differences compared to the intersection scenarios often found in the literature. For instance, the full site-layout of confined areas is typically known at the planning stage, and it can often be expected that no non-controlled actors will disturb the execution of a plan once its formed. The motion of each vehicle can therefore be planned from the start of a transport mission to its end. Planning for confined sites is therefore benefited by methods that can handle long planning horizons. This in contrast to the intersection coordination context found in the literature, where a cutout around the intersection proper is most often considered, with vehicles arriving at speed at the intersection cutout [6], [7]. Moreover, in confined areas, the motion plan must consider coordination of the vehicles’ use of a number of resources besides intersections, e.g., different variety of collision zones, and the quality of the plan has a direct and significant impact on the profit margins of the transport solution provider.

[0007] Direct optimal control is contemplated as a favorable type of method for solving the optimization problem. In the present disclosure, a two-stage heuristic for obtaining the optimal crossing order for each collision zone is utilized. The idea proposed in [9] is adapted to suit the goals in this application. The problem is formed in the space frame as it allows for optimizing the motion over the full vehicle path, as well as, allowing a more convenient handling of the collision constraints that occur in the collision zones. Furthermore, the collision constraints for the merge-split collision zone are defined such that the zone is not excluded for only one vehicle at a time, and the algorithm is modified to be able to handle these zones along with being able to handle multiple collision zones between vehicles. Although the approach focuses on confined sites, the method of handling the mutual exclusion zones is extendable to other scenarios as well (e.g., public road applications).

[0008] From similar scenarios that have been considered, the authors in [11] and propose an optimization approach for handling merge scenarios, which is a subset of the merge-split collision zone and [12] uses a game theoretical strategy for optimizing traffic flow through multiple intersection collision zones. The handling of multiple intersections and zones of different types were also identified in the survey [2] as topics for further work in this field.

SUMMARY

[0009] One objective of the present disclosure is to make available more widely applicable vehicle coordination techniques. A further objective is to enable planning over the entire site (a full plan covering the entire navigable environment of the vehicles) with at most a feasible effort increase. A further objective is to enable planning at all moments with at most a feasible effort increase. In particular, it is desired to remove the time horizon of the OCP and allow it to extend to the end of each vehicle’s assigned transport mission. A further object is to enable planning for environments with multiple conflict zones (CZs). A further object is to enable planning for environments with at least one dwelling zone (e.g., charging, tipping, servicing), which is such that special vehicle dynamics apply to any vehicle therein, and/or at least one merge-split zone.

[0010] At least some of these objectives are achieved by the invention as defined by the independent claims. The dependent claims relate to embodiments of the invention.

[0011] In a first aspect of the invention, there is provided a method of planning routes for a plurality of vehicles operating in a common environment which includes at least one CZ, wherein movements of each vehicle are controllable by a control signal. The method comprises: obtaining a predefined objective function J; solving a first optimization problem for a first objective function V _i (T _i ) derived from the predefined objective function, to obtain a vehicle crossing order at each CZ, wherein the first optimization problem is subject to safety constraints; and solving an optimal- control problem (OCP) for the predefined objective function subject to the vehicle crossing order at the CZs and subject to the safety constraints, to obtain a control signal u _i for each of the vehicles. The OCP includes a dynamic vehicle model representing evolution with respect to path length p _i in which time t _i (p _i ) and path speed v _i (p _i ) are state variables.

[0012] The invention according to the first aspect enables the following advantages compared with the prior art. Because the OCP is formulated with path length (distance along the path) as the independent variable, rather than time, it becomes possible to plan over the entire site at all moments without significant effort increase. The use of path length also avoids certain numerical difficulties which affected the prior art, relating to requirements for computational resource and the complexity in formulating the OCP. Further, it becomes possible to handle CZs of other types than intersections, namely, the merge-split combination, charging stations, loading/unloading stops, tunnels and other single-lane passages. It becomes possible to account for all CZs present at a site. The use of path length as independent variable further allows the site state and topography (e.g., the road slope, the friction conditions on different parts, etc.) to be accounted for.

[0013] In a second aspect of the present invention, there is provided a method of controlling a plurality of vehicles operating in a common environment which includes at least one CZ, wherein movements of each vehicle is controllable by a control signal, the method comprising: sensing current positions P of all vehicles; obtaining a predefined objective function J; solving a first optimization problem for a first objective function V _i (T _i ) derived from the predefined objective function, to obtain a vehicle crossing order at each CZ, wherein the first optimization problem is subject to safety constraints and subject to the sensed vehicle positions; solving an optimal- control problem (OCP) for the predefined objective function subject to the vehicle crossing order at the CZs and subject to the safety constraints, to obtain a control signal u _i for each of the vehicles; and feeding the control signal to the vehicles.

[0014] In a third aspect of the present invention, there is provided a device comprising memory and processing circuitry which is configured to perform the above method of planning routes for a plurality of vehicles operating in a common environment which includes at least one CZ, or to perform the above method of controlling these vehicles. The invention further relates to a computer program containing instructions for causing a computer, or the device in particular, to carry out the above methods. The computer program maybe stored or distributed on a data carrier. As used herein, a “data carrier” may be a transitory data carrier, such as modulated electromagnetic or optical waves, or a non-transitory data carrier. Non- transitory data carriers include volatile and non-volatile memories, such as permanent and non-permanent storage media of magnetic, optical or solid-state type. Still within the scope of “data carrier”, such memories may be fixedly mounted or portable.

[0015] The second and third aspects of the invention generally share the effects and advantages of the first aspect, and they can be implemented with an equal degree of technical variation.

[0016] The term “conflict zone” may include a multiple-exclusion (MUTEX) zone.

[0017] The term “path length” refers to the vehicle’s position constrained to a predefined path. As such, even though the vehicle is designed for two-dimensional motion, the path length is a one-dimensional quantity, which can be represented, e.g., as the distance p _i from a reference point on the path. Similarly, path speed v _i can be understood as the time derivative of the path length.

[0018] The term “route” may refer to a representation indicating a vehicle’s position (e.g., a point in space) at different times. A “path” may refer to the set of positions that a vehicle has traversed or will traverse, without indicating when this occurred or will occur. As such, a route may be considered to be a function from time to space, whereas a path is a static quantity.

[0019] Generally, all terms used in the claims are to be interpreted according to their ordinary meaning in the technical field, unless explicitly defined otherwise herein. All references to “a/an/the element, apparatus, component, means, step, etc.” are to be interpreted openly as referring to at least one instance of the element, apparatus, component, means, step, etc., unless explicitly stated otherwise. The steps of any method disclosed herein do not have to be performed in the exact order described, unless explicitly stated.

[0020] Embodiments of the disclosed invention may enable one or more of the following:

• State explicit performance objectives and ability to determine how these should be traded off between each other.

• State explicit system constraints (e.g., individual transport mission deadlines, speed and acceleration limits etc.).

• Include explicit safety constraints with timable margins. • Incorporate physical constraints and effects (e.g., the vehicle dynamics and propulsion system efficiency, including the effect increased energy consumption induced by uphill driving, or utilize the potential energy acquired by a vehicle at a hill).

• Incorporate the energy state of a vehicle to automatically determine when a BEV should be charged.

• Introduce explicit priorities among the vehicles.

• Introduce feedback: the traffic plan can be recomputed at a high rate to handle deviations from the current plan.

• Have flexibility in tuning, all the above can be changed on-the fly, leading to situations where the overall system behavior can be changed at-will (e.g., changing prioritization between throughput and energy efficiency depending on customer needs).

• Perform site-wide optimization considering complete vehicle trajectories and the site topology and all conflict zones, simultaneously.

BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Aspects and embodiments are now described, by way of example, with reference to the accompanying drawings.

[0022] Figure 1 illustrates two types of CZs, namely, an intersection CZ (figure 1A) and a merge-split CZ (figure 1B).

[0023] Figure 2 is an illustration of the heuristic approach. As seen, in figure 2A the original problem is a non-convex problem with a disconnected solution space (each compact subset corresponding to an integer combination), whose solution is, typically, difficult to obtain. Figure 2B shows the MIQP approximation of the original problem, which approximation allows for finding a region where the solution of the original problem could be. Finally, through the fixed-order NLP (see figure 2C), the approximate optimal solution is acquired.

[0024] Figure 3 shows a mock-up confined site area together with a legend of the line styles to be used in the subsequent figures. [0025] Figure 4 shows uncoordinated crossing for the merge-split CZ for the red vehicle (solid curve) and blue vehicle (dashed curve).

[0026] Figure 5 shows coordinated crossing for the merge-split CZ for the red vehicle (solid curve) and blue vehicle (dashed curve).

[0027] Figure 6 shows uncoordinated crossing of an intersection CZ for the red vehicle (solid curve) and green vehicle (dash-dotted curve).

[0028] Figure 7 shows coordinated crossing of an intersection CZ for the red vehicle (solid curve) and green vehicle (dash-dotted curve).

[0029] Figure 8 shows speed profiles for the coordinated vehicles (from top to bottom: red, blue, green, black).

[0030] Figure 9 shows the time gap between the red and blue vehicles in the merge-split CZ for the coordinate and uncoordinated cases.

[0031] Figure 10 shows uncoordinated and coordinated crossing of the inter- section zone. An intersection of the depicted trajectory with the intersection zone is equivalent to both vehicles being in the CZ at the same time.

[0032] Figure 11 shows uncoordinated crossing for the merge-split CZ for the red and blue vehicle. The grey box is the occupancy time in the zone for the red vehicle. While it is allowed for the blue vehicle to enter the CZ whilst the red vehicle is in it, an intersection between the vehicles translates to a collision between the vehicles

[0033] Figure 12 shows coordinated crossing for the merge-split CZ for the red and blue vehicle. The grey box is the occupancy time in the zone for the red vehicle. The blue vehicle enters the CZ whilst the red vehicle is in and keeps the desired time gap.

[0034] Figure 13 is a flowchart of a method for planning routs of a plurality of vehicles, according to an embodiment of the invention.

[0035] Figure 14 shows an environment with multiple intersection CZs, merge- split CZs and dwelling-type CZs, in which a plurality of vehicles operates, as well as a device suitable for controlling said vehicles is deployed.

[0036] Figure 15 is a sequence diagram showing an exchange of signals between a vehicle and a device configured to control a group of vehicles to which the vehicle belongs. DETAILED DESCRIPTION

[0037] The aspects of the present disclosure will now be described more fully hereinafter with reference to the accompanying drawings, in which certain embodiments of the invention are shown. These aspects may, however, be embodied in many different forms and should not be construed as limiting; rather, these embodiments are provided by way of example so that this disclosure will be thorough and complete, and to fully convey the scope of all aspects of the invention to those skilled in the art. Like numbers refer to like elements throughout the description.

Problem formulation

[0038] For the confined site coordination problem, N _a vehicles that need to be coordinated efficiently through different types of exclusion zones without collisions are considered. Assuming a confined site means that no unexpected traffic participants are present, i.e., no other vehicles than the planned ones, no pedestrians, etc. Furthermore, all coordinated vehicles are assumed to be automated, capable to communicate, their full paths and common exclusion zones are known a priori and overtakes does not occur.

[0039] Figure 14 shows an environment 1400 in which a plurality of vehicles 1460 operate. The environment 1400 includes roads 1430 as well as multiple intersection CZs 1410, merge-split CZs 1410 and dwelling-type CZs (dwelling zones) 1420. The environment 1400 maybe a confined area with no other traffic participants than the vehicles 1460. The vehicles 1460 maybe conventional vehicles, partially autonomous vehicles or fully autonomous vehicles; they may be cars, trucks, buses, construction equipment; they may be single vehicles or multi-unit vehicle combinations. Also visible in figure 14 is a device 1450 suitable for controlling said vehicles 1460. The device 1450 includes processing circuitry 1452 and is configured to communicate over a wireless interface 1451 with corresponding wireless interfaces 1461 on the vehicles 1460. As indicated for one example vehicle 1460, the vehicles 1460 circulating in the environment 1400 may further include vehicle-carried processing circuitry 1462. 10

Vehicle model

[0040] The motion of the vehicles along their path is described by where p _i (t) ∈ is the position, x _i (t) E the vehicle state, u _i (t) ∈ the control input, and i is a vehicle index. The state is subdivided as x _i (t) = v _i (t),z _i (t)), with the speed along the path v _i (t) ∈ and z _i (t) ∈ collecting possible other states. The functions f _i and h _i describe respectively the dynamics and constraints that capture, e.g., actuator and speed limits. Both functions are assumed to be smooth.

Vehicle model in space

[0041] For confined site optimization a main interest is to optimize the trajectories of the vehicles over the whole site, i.e., over their full paths. However, the time it takes a vehicle to traverse a path is unknown. Therefore, the necessary time horizon is undefined, making the coordination problem difficult to formulate with time as the independent variable. Due to this, the problem is reformulated "in space", using that

[0042] The spatial dynamics are

(4, 5, 6) where the position p _i is the independent variable.

Conflict zone modelling

[0043] A conflict zone (CZ) is described by the entry and exit position on the path of each vehicle. From the known positions, the entry and exit times through the CZ can be defined. In the present disclosure, two types of conflict zones are considered as depicted in Figure 1. Let denote a set of all intersections (total number: r ₀ ) in the confined site and Q _r = {q _r , ₁ , q _r , ₂ , ... , q _r , _l } denote the set of vehicles that cross intersection l _r . In the intersection-like CZ, it is desired to only have one vehicle inside the CZ, i.e., not allowing the vehicle j to enter the CZ before vehicle i + j exits the CZ, or vice-versa. The order in which the vehicles cross the intersection I _r is denoted ’ where s _r , ₁ , s _r , ₂ ,... are vehicle indices and A sufficient condition for collision avoidance for the r-th intersection CZ can be formulated as where t is determined from (4).

[0044] In the merge-split CZ case, let denote a set of all merge-split zones (total number: w ₀ ) in the site and Z _w = {z _{w 1} , z _w,2 , .... z _w,h } denote the set of vehicles that cross the merge-split CZ M _w . It is desired to have the vehicles in the zone at the same time, instead of blocking the whole zone, and thus increasing throughput efficiency. This requires having rear-end collision constraints once the vehicles have entered the CZ safely. In this case, the order in which the vehicles enter the zone is denoted as and . The collision avoidance requirement for this w-th CZ leads in having to enforce the following constraints:

[0045] The time At above is added for an additional constant time gap. However, only including a time gap would lead to the problem that at low speeds the vehicles would be very close to each other, unless At is not very large. Large Δt, on the other hand, would lead to too large distance between the vehicles at high speeds. Therefore, a constant c is included as well. Assuming that the vehicles do not reverse, the velocity of a moving vehicle is larger than zero, meaning c > 0. Optimal Control Problem

[0046] A direct optimal control approach is used to solve the problem. The independent variable is discretized as p _i = (p _i , ₁ ,...,p _i , _Ni ), the input approximated using zero order hold such that and equations (4, 5) are (numerically) integrated on this grid, giving the "discretized" state transition relation where F denotes the integration of (4, 5) from p _i,k to P _i,k+ . Using this, the problem of finding the optimal vehicle trajectories that avoid collisions can be formalized as:

[0047] (Optimal Coordination Problem) Obtain the optimal state and control trajectories X given the initial state by solving the optimization problem (10a) subject to initial guess (10b) system dynamics (9) (10c) state and input constraints (6) (10d) safety constraints (7), (8). (10e)

[0048] Note in particular that this involves finding the crossing orders which makes the problem combinatorial and difficult to solve.

Method

[0049] Problem (10) can be solved as Mixed Integer Nonlinear Program (MINLP). However, finding a solution to such problems is known to be difficult, especially when the constraints or the objective function are non-convex. Therefore, a common procedure is to apply an approach where the integer part of the solution is obtained first using a heuristic, and the continuous part of the solution thereafter obtained by solving the nonlinear program (NLP) that results from fixing the integers to the values found with the heuristic. This approach is followed in the present disclosure. The heuristic that is used is similar to that of [9] and approximates the integer solution of (10) by solving a Mixed Integer Quadratic Problem (MIQP). With the integer solution given, the state and control trajectories are obtained by solving the “fixed-order coordination” NLP, i.e., Problem (10) with fixed crossing orders

Crossing order heuristic

[0050] The crossing order heuristic follows the procedure proposed in where the coordination problem is approximated as time-slot scheduling MINLP in T, where T = (T _i ,...,T _Na ) and T _i the set of entry and exit times for vehicle i for all the CZ it encounters. With this formulation, the integer part of the solution, i.e., the crossing orders, are treated in T space. The problem reads: subject to T where dom(V _i ) is the domain of the function V _i . The definition of follows next.

[0051] The Vehicle problem: The function V _i (T _i ) is the optimal value function of the following parametric optimal control problem, denoted the “Vehicle Problem":

(12a) subject to (6), (9), (10b) (12b) with being the parameter over which optimization is performed. Note that dom(V _i ) thereby is the set of parameters T _i for which (12) has a solution.

[0052] An MIQP-based heuristic: The time-slot scheduling MINLP (11) is still a difficult problem, with non-convex objective function and constraints. Motivated by the availability of efficient solvers, the following MIQP approximation of (11) is considered: subject to (7), (8), (13b) where the derivatives are evaluated at , which is obtained from the first step of Algorithm 1 below. Note that besides using the first and second order Taylor expansion of the objective function and constraints, respectively, the constraint dom(V _i ) is removed. This is motivated by the conclusions in [9].

[0053] The solution to the MIQP problem provides an approximately optimal time-slot schedule T and therefore an approximate crossing orders

[0054] A comment on differentiation of V _i (T _i ) : From the solution of the parametric NLP (12), the first and second order derivatives w.r.t. the parameter can be calculated using the parametric sensitivity analysis [14]. In particular, this can be done at a low additional cost when the optimal problem is solved. Note that for a solution to problem (12), feasible entry and exit times, i.e., feasible values for the parameters, for each CZ that vehicle i encounters are necessary. These times can, for example, be computed by solving an optimization problem without the safety constraints.

Fixed-order NLP

[0055] With the found crossing order, the integer part of the solution is obtained, making (10) a NLP. Obtaining the optimal state and control trajectories is thus found through solving the fixed-order coordination problem subject to (10b), (10c), (10d), (10e) (14b) [0056] The two stage approximation approach is summarized in Algorithm 1 and depicted in Figure 2.

ALGORITHM 1

Input: vehicle paths

Output:

1. For all i, solve NLP (10) without safety constraints (7), (8) to obtain feasible entry and exit times T _i ^[0]

2. For all i, solve NLP (12) and calculate the first- and second-order sensitivities

3. Solve MIQP (13) for the approximate crossing orders

4. Solve the fixed-order NLP (14) using and obtain

[0057] In this section, a simulation example showing the operation and performance of the coordination algorithm will be presented.

Simulation setup

[0058] The vehicles are modelled as a triple integrator x = u, whereby the spatial model is: where a _i is the acceleration and u _i is the jerk. The dynamics are discretized using multiple shooting and an Explicit Runge-Kutta-4 (ERK4).

[0059] The constraints are chosen as bounds on the speed and longitudinal acceleration in order to obey speed limits and physical constraints. The vehicles are expected to manoeuvre on curved roads, thus, it is needed to consider the lateral forces they experience. As the one-dimensional model that is used in this disclosure does not account for lateral motion, the following constraint is enforced, as similarly proposed in [8]: where is the lateral acceleration limit and K _i (p _i,k ) is the road curvature, that is assumed to be available at every point along the path.

[0060] In this disclosure, it is of interest to minimize the change of acceleration and jerk in order for the vehicles to operate in a comfortable way. Minimizing the acceleration and jerk would also lead to a reduction in fuel/energy consumption. This requirement leads to the following objective function for each vehicle: with the appropriate weights and being the final time.

[0061] For evaluation purposes, a mock-up confined area with four vehicles as depicted in figure 3 will be investigated. There are in total two merge-split CZs and five intersection CZs. Every vehicle starts from an initial velocity of 50 [km/h] and the chosen bounds are

2 [m/s ² ]. The weight coefficients for the optimal problem are: 10. The total shooting points for this scenario is N = 100 and is same for all vehicles. In the intersection-like scenario the CZ is created with 5 meter margin ahead of and behind the collision point, whereas in the merge-split scenario the margin is 15 meters for both the entry and exit collision point. For the merge-split CZ it is also desired to keep at least 0.5s margin between the vehicles.

[0062] The Casadi [18] toolkit with the IPOPT [19] solver will be utilized for solving the optimization problems (12, 14) and Gurobi for solving the MIQP problem (13)-

Discussion of results

[0063] In the following, the results for this simulation scenario are presented and, in particular, the uncoordinated and coordinated results for one merge-split zone and one intersection zone, both circled over in figure 3. The uncoordinated results are obtained as the individual optimum of each vehicle, i.e., the speed profiles are obtained by solving the optimization problem without any safety constraints.

[0064] Figures 11 and 12 illustrate the uncoordinated and coordinated crossing of the merge-split CZs, respectively. In the figures, the path trajectories of the involved vehicles are depicted. The path trajectories have an offset of their entry position to the merge-split CZ, meaning that, for both vehicles, position of zero indicates the entry position to the CZ. The grey rectangle depicts the CZ, with entry and exit times for the red vehicle, since that is the first vehicle that enters the zone in both cases. As mentioned, in this type of CZ it is desired to have multiple vehicles inside the zone whilst keeping a minimum gap between them. This means that while it is allowed for both trajectories to be inside the zone, it is not allowed for the trajectories to intersect whilst in the zone. In the uncoordinated case, it can be noticed that the blue vehicle enters the CZ later than the red vehicle, however, intersects the red vehicle and exits the zone first. The interpretation of this behavior is that inside the CZ the blue vehicle "overruns" the red vehicle, meaning that a collision occurs. In the coordinated case, the vehicles are capable of being inside the CZ at the same time and keeping at least a minimum designated gap between each other. Note that an intersection of the trajectories outside the CZ is not relevant as they are no longer on a shared road. By allowing the vehicles to be both in the CZ, and not blocking the whole zone for one vehicle, the throughput is increased meaning that the vehicles could get to their end destinations sooner. This could translate to increased efficiency of the site. To supplement the figures, figure 9 depicts the time gap between the vehicles while they are in the CZ. In the figure, the moment where the blue vehicle "overruns" the red vehicle in the uncoordinated case can be noticed, as well as the fact, that the desired minimum gap is kept for the coordinated case.

[0065] In the scenarios where two (or more) vehicles intersect, it is desired to only have one vehicle in the zone. The reasoning for this decision is that the CZ in this case occupies a relatively small patch of road and blocking off the whole zone for one vehicle will not result in major loss of throughput efficiency. Figure 10 depicts the uncoordinated and coordinated vehicle trajectories along with the intersection zone. For this dependency, a collision is defined if the trajectory intersects the intersection zone, which is equivalent to the two vehicles being in the zone at the same time. It is noticeable that in the uncoordinated case the intersection zone is intersected, whilst the coordinated vehicles satisfy the collision constraints as they successfully avoid being in the CZ at the same time.

[0066] Figure 8 depicts the vehicles’ speed profiles throughout the site. As the objective function is to minimize the change of acceleration and jerk, it is noticeable that the speed profiles are smooth throughout the whole site and would not be challenging to follow in an application case. In the speed plots, the solid black lines represent the curvature constraints.

[0067] It is worth noting that in the uncoordinated case for the whole site, two more collisions occur. One in the other merge-split CZ between the blue and green vehicle and another in the intersection CZ of the blue and black vehicle. When the vehicles are coordinated, these collisions are avoided. For sake of brevity, these results will not be depicted here, as the critical behavior is similar to the above- mentioned collision zones.

[0068] The simulation scenario is implemented in MATLAB on a 2.90GHz Intel Xeon computer with 32GB of RAM. The total computational time for solving the example is 40.6313 seconds. However, the main computational load comes from the Casadi toolkit and setup. To be precise, the total time all solvers take is 1.815 seconds, of which the MIQP requires 0.08 seconds and the main Optimal Control Problem (OCP) 0.709 seconds and the remainder is the required time to solve the individual vehicle problem. The computational effort besides the solver time is the time Casadi takes to set up the environment. The total solver time is, however, still fairly high especially for real-time vehicle application and needs to be further improved.

Conclusions and future theoretical work

[0069] The present disclosure has presented an optimal control based approach for coordination of automated vehicles for sites where multiple collision zones of different types could occur. The approach optimizes the trajectories of the vehicles for their whole path length, taking all collision zones into account. To compute the crossing order in the collision zones, the approach relies on an MIQP-based heuristic. Simulation results demonstrate the capability to coordinate the vehicles without collisions. Implem entationsand performance

[0070] Figure 13 is a flowchart of a method 1300 for controlling a plurality of vehicles 1460 (see figure 14) operating in a common environment 1400 which includes at least one CZ, wherein movements of an i ^th vehicle is controllable by a control signal u _i . The execution of the method 1300 maybe coordinated by a central processing resource being either fixedly installed or vehicle-carried. In the following description, by way of example, it will be assumed that the execution of the method 1300 is coordinated by processing circuitry 1452 fixedly installed in the device 1450. The device 1450 can delegate the execution of some steps of the method 1300, or parts thereof, to vehicle-carried processing resources (e.g., processing circuitry 1462) by transmitting a corresponding delegation signal D to the vehicles 1460 over the wireless interface 1451 and receiving a processing result message E in return.

[0071] In a first step 1310 of the method 1300, current positions of all vehicles are sensed. The vehicle positions maybe self-reported positions represented by a signal P (see figure 15) transmitted from each vehicle 1460 to the device 1450. The transmission of the signal P may be event-triggered, periodic or triggered by a request (not shown) transmitted from the device 1450 to the vehicles 1460.

[0072] In a second step 1320, a predefined objective function J is obtained.

[0073] In a third step 1330, a first optimization problem for a first objective function derived from the predefined objective function is solved, to obtain a vehicle crossing order at each CZ, wherein the first optimization problem is subject to safety constraints. The safety constraints for an intersection zone 1410 or a dwelling zone 1420 may include a mutual exclusion requirement. The safety constraints for a merge-split zone 1410 may include a minimum longitudinal spacing requirement, Δt or Δt + c.

[0074] The CZ entry and CZ exit times may be decision variables of the first optimization problem. Further, the first objective function can be a parametric local optimum of the predefined objective function J, wherein the parametric local optimum is parametrized by tentative CZ entry and CZ exit times and is independent of the safety constraints. In this case, the tentative CZ entry and CZ exit times are decision variables in the first optimization problem. [0075] Optionally, the first objective function is approximated 1334 by a polynomial, such as a quadratic polynomial. In particular, the polynomial maybe a Taylor expansion around an approximate set of CZ entry and CZ exit times; see equation 13a. The approximate set of CZ entry and CZ exit times maybe obtained 1332 by a preceding step of optimizing the predefined objective function independently of the safety constraints. The optimization problem expressed in terms of the polynomial maybe solved as a mixed-integer quadratic program (MIQP).

[0076] In a fourth step 1340 of the method 1300, an OCP for the predefined objective function is solved subject to the vehicle crossing order at the CZs and subject to the safety constraints, to obtain a control signal u _i for each of the vehicles. It is understood that OCP includes a dynamic vehicle model representing evolution with respect to path length p _i in which time and path speed are state variables.

[0077] In a fifth step 1350, the control signals u _i are fed to the vehicles (signal U in figure 15) for execution.

[0078] In one embodiment, a subset comprising only the obtaining 1320, solving

1330, and OCP solving 1340 steps of the method 1300 can be implemented in isolation, for the purpose of planning routes for said plurality of vehicles 1460.

[0079] The invention can be embodied as a software component that returns motion plans (e.g. speed profiles) for all vehicles from their current position to the end of their mission. The core of this software component is the generation of the motion plans, done such that no collisions occur and such that the physics of reality is considered. As seen above, the motion plans are generated by (approximately) solving an optimization problem. This optimization problem is formulated and solved based on an optimal control formulation of the site-coordination problem, using procedures from what is known as Numerical Optimal Control (NOC). NOC is a field where mathematical optimization is used to compute control commands for a dynamical system (e.g. a mechanical system such as trucks/machines) such that its evolution optimizes a performance criterion while satisfying certain constraints.

[0080] The solution to the problem yields the motion profiles for each involved vehicle, from its current position on the site to the terminal point of its transport mission, which • are consistent with what the vehicles can do given the available knowledge of the environment and the limitations imposed by the designer (satisfy the vehicle dynamics and the vehicle constraints)

• are free of collisions and satisfies MUTEX requirements.

• maximizes performance in terms of the metric specified under the two conditions above.

[0081] As regards computational tractability, the following can be noted. In contrast to OCPs found in most other fields, the site coordination problem is combinatorial, as the order in which the vehicles on the site utilize the CZs (or MUTEX zones) must be determined to produce a solution. This means that the size of the space of possible solutions grows exponentially and that its solution therefore is computationally intractable for anything but small problems, in the general case. For illustration, consider a k-way crossing with n _j vehicles approaching along a j-th lane. The total number of crossing combinations are

For a total of 8 vehicles, all approaching a four-way intersection with two from each direction, there would be 2520 distinct solutions, differing in which order the vehicles use each CZ (the crossing order). As the number of CZs and vehicles increase, the number of solutions grows rapidly.

[0082] Algorithms to handle this type of problems typically operate by systematically ruling out large parts of the possible crossing orders. This process involves solving so-called “relaxations” of the original problem. These relaxations are not combinatorial in themselves but otherwise inherits their complexity-class from the non-combinatorial part of the original problem. In the case of site-coordination problems, these are non-convex, nonlinear programs (NCNLPs), which typically are non-trivial to solve, even though so called local solutions often can be obtained. This means that, for a typical problem, many relaxations must be obtained to find a solution, and this number grows with the number of possible crossing orders in the original problem’s solution-space. Hence, with the site coordination problems considered, most real problems become practically unsolvable in the context within they must be used, as the plan will be irrelevant once it is obtained. [0083] This is one of the reasons for which the invention includes an approximating separation, performed in two steps:

(1) The construction of the “combinatorial part” of the solution through a “site- aware” heuristic, i.e., for each CZ the determination of which vehicle goes first, second, third etc., within the set of contending vehicles, taking into account the vehicle limitations, the site topology etc. approximately; and

(2) the solution of a site-wide optimal control problem where the MUTEX order is fixed. This is a standard OCP and computationally tractable optimization problem, solvable with commercial tools within the time limits admitted by the usage context, whose solution gives the optimal motion profiles for all vehicles given the current vehicle state and the heuristically obtained MUTEX order.

Here, the site-aware heuristics may include two stages:

• (automatically) simplified representations of the performance objectives, physical constraints and vehicle constraints; and

• a formulation of a simplified representation of the problem, using these simplifications, as a Mixed-Integer Quadratic Program (MIQPs).

Indeed, while the size of the combinatorial solution space remains the same (e.g., the number of distinct ways the 8 vehicles could cross the intersection in the example above), there are several well developed commercial tools to handle MIQPs efficiently. This allows the invention to handle significantly larger (and therefore relevant) problems within the timespans admitted by the intended usage context. The excellent scalability is one of the prominent features of the invention, allowing it to handle coordination of a desired number of vehicles in real time or near real time conditions.

[0084] The principal steps of the invention can be summarized as follows :

1. The central site planner obtains the current position and velocity of each automated vehicle on the site via wireless communication.

2. The central site planner finds a crossing order by a site-aware heuristic based on the solution of a Mixed-Integer Quadratic Program (MIQP). a. If the MIQP solution fails, the planner sets the crossing order of the previous cycle to be the current. 3. The central site planner solves an OCP for all vehicles where the vehicles are positioned according to the measurements obtained in step 1 and the crossing order is that produced in step 2. a. If the OCP solution fails, the planner sets a system fault and produce plans for all vehicles to stop immediately.

4. The central planner distributes the plan produced in step 3 to all vehicles.

[0085] Already on this general level, the invention differs from [6] notably in that the non-approximated problem, and therefore the OCP solved in step 3, is formulated with path length (distance along the path) as the independent variable, rather than time. This enables planning over the entire site (the full plan) at all moments without significant effort increase and avoids certain numerical difficulties, both issues preset in [6]. If one were to follow the lines of [6], severe penalties arise in one or both of computational resource requirements and problem formulation complexity.

Notionally ‘using space rather than time* as the independent variable is therefore an important step towards being able to treat site planning problems with optimization, which is not trivially realized.

[0086] Under step 2, the site-aware heuristic may operate as follows:

• For each automated vehicle, compute a quadratic approximation of that vehicle’s optimal performance metric as a function of the previous CZ timeslot schedule (initialized by e.g. first-come first-served), this amounts to so solving one small OCP (‘greedy* problem) for each vehicle and applying tools from parametric sensitivity analysis.

• Attempt to solve, using the central site planner, an MIQP constructed with the quadratic approximations from a). This gives a tentative CZ schedule.

• From the CZ schedule, extract the crossing order.

The MIQP is thus constructed on a high-level abstraction of the original OCP (the problem of finding the optimal time-slot schedule) and does not deal with the positions or velocities of the vehicles directly. The resulting MIQP has small relaxations but necessitates the solution of one small OCP per vehicle.

[0087] With regard to step 4, it is noted that the plan produced by the central site planner need not be followed precisely by the vehicles on the site. If this is the case, replanning is necessary. Embodiments of the invention may therefore include feedback by periodically solving the site wide OCP based on fresh measurements of the system state. Also, the crossing order could be updated using a local search heuristic, or re-computed from scratch with either of the two heuristics.

[0088] The above-described solution can be parallelized in one or more of the following ways:

• Parallel computation of each vehicle’s cost and quadratic approximation. Central solution of MIQP and subsequent solution of site wide OCP.

• On-board computation (i.e., using processing resources carried by the vehicle) of each vehicle’s OCP, data is sent to a server which solves the MIQP site wide OCP.

• On-board computation of each vehicle’s OCP, data is sent to server which computes the MUTEX utilization order, server and vehicles solve the problem cooperatively, distributed computations.

• On-board computation of each vehicle’s OCP, data is sent vehicle to vehicle, MUTEX utilization order is negotiated vehicle to vehicle, decentralized computation.

• Site-wide OCP: The solution can be parallelized by use of tailored linear- algebra which leverages the numerical structure of the problem. For instance, parts of the computations needed to solve a common problem can be delegated by the central site planner to processing resources carried by the vehicles. This may relate to computationally offloading the central site planner without necessarily ensuring that the delegation causes each vehicle to carry out ‘its own’ computations.

[0089] The aspects of the present disclosure have mainly been described above with reference to a few embodiments and examples thereof. However, as is readily appreciated by a person skilled in the art, other embodiments than the ones disclosed above are equally possible within the scope of the invention, as defined by the appended patent claims. REFERENCES

Bagloee, Saeed Asadi, Madjid Tavana, Mohsen Asadi, and Tracey Oliver. "Autonomous vehicles: challenges, opportunities, and future implications for transportation policies.” Journal of modem transportation 24, no. 4 (2016): 284-303.

2. Khayatian, Mohammad, Mohammadreza Mehrabian, Edward Andert, Rachel Dedinsky, Sarthake Choudhary, Yingyan Lou, and Aviral Shirvastava. "A survey on intersection management of connected autonomous vehicles.” ACM Transactions on Cyber-Physical Systems 4, no. 4 (2020): 1-27.

3- Hafner, Michael R., Drew Cunningham, Lorenzo Caminiti, and Domitilla Del Vecchio. "Cooperative collision avoidance at intersections: Algorithms and experiments.” IEEE Transactions on Intelligent Transportation Systems 14, no. 3 (2013): 1162-1175.

4- Kuyer, Lior, Shimon Whiteson, Bram Bakker, and Nikos Vlassis. "Multiagent reinforcement learning for urban traffic control using coordination graphs.” In Joint European Conference on Machine Learning and Knowledge Discovery in Databases, pp. 656-671. Springer, Berlin, Heidelberg, 2008.

5- Colombo, Alessandro, and Domitilla Del Vecchio. "Least restrictive supervisors for intersection collision avoidance: A scheduling approach.” IEEE Transactions on Automatic Control 60, no. 6 (2014): 1515-1527.

6. Hult, Robert, Mario Zanon, Sebastien Gros, and Paolo Falcone. "Optimal coordination of automated vehicles at intersections: Theory and experiments.” IEEE Transactions on Control Systems Technology 27, no. 6 (2018): 2510- 2525.

7- Katriniok, Alexander, Peter Kleibaum, and Martina Jos ^v evski. "Distributed model predictive control for intersection automation using a parallelized optimization approach.” IFAC-PapersOnLine 50, no. 1 (2017): 5940-5946.

8. Hult, Robert, Mario Zanon, Se'bastien Gros, and Paolo Falcone. "Optimal coordination of automated vehicles at intersections with turns.” In 2019 18th European Control Conference (ECC), pp. 225-230. IEEE, 2019. 9- Hult, Robert, Mario Zanon, Sebastien Gras, and Paolo Falcone. "An miqp- based heuristic for optimal coordination of vehicles at intersections.” In 2018 IEEE Conference on Decision and Control (CDC), pp. 2783-2790. IEEE, 2018.

10. Letter, Clark, and Lily Elefteriadou. "Efficient control of fully automated connected vehicles at freeway merge segments.” Transportation Research Part C: Emerging Technologies 80 (2017): 190-205.

11. Omidvar, Aschkan, Lily Elefteriadou, Mahmoud Pourmehrab, and Clark Letter. "Optimizing freeway merge operations under conventional and automated vehicle traffic.” Journal of Transportation Engineering, Part A: Systems 146, no. 7 (2020): 04020059.

12. Bui, Khac-Hoai Nam, and Jason J. Jung. "Cooperative game-theoretic approach to traffic flow optimization for multiple intersections.” Computers & Electrical Engineering 71 (2018): 1012-1024.

13- Richards, Arthur, and Jonathan How. "Mixed-integer programming for control.” In Proceedings of the 2005, American Control Conference, 2005., pp. 2676-2683. IEEE, 2005.

14. Fiacco, Anthony V. Introduction to sensitivity and stability analysis in nonlinear programming. Vol. 165. Academic press, 1983.

15- Colombo, Alessandro, and Domitilla Del Vecchio. "Efficient algorithms for collision avoidance at intersections.” In Proceedings of the 15th ACM international conference on Hybrid Systems: Computation and Control, pp. 145-154. 2012.

16. Kobetski, Avenir, Domenico Spensieri, and Martin Fabian. "Scheduling algorithms for optimal robot cell coordination-a comparison.” In 2006 IEEE International Conference on Automation Science and Engineering, pp. 381- 386. IEEE, 2006.

17- Blaz’ewicz, Jacek, Wolfgang Domschke, and Erwin Pesch. "The job shop scheduling problem: Conventional and new solution techniques.” European journal of operational research 93, no. 1 (1996): 1-33. 18. Andersson, Joel AE, Joris Gillis, Greg Hom, James B. Rawlings, and Moritz Diehl. "CasADi: a software framework for nonlinear optimization and optimal control.” Mathematical Programming Computation 11, no. 1 (2019): 1-36.

19. Wachter, Andreas, and Lorenz T. Biegler. ”On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming.” Mathematical programming 106, no. 1 (2006): 25-57.