EP4599418A1 - System and method for controlling an autonomous vehicle in a multi-lane driving environment - Google Patents

Abstract

Method of controlling an ego vehicle in a multi-lane driving environment, comprising: sensing position data of one or more surrounding vehicles, which are included a multi-lane driving environment; planning a trajectory of the ego vehicle in the multi-lane driving environment; predicting trajectories of the surrounding vehicles for at least the duration of the trajectory of the ego vehicle, wherein the prediction is based on the sensed position data of the surrounding vehicles; updating, at least once, the trajectory of the ego vehicle in view of the predicted trajectories of the surrounding vehicles; and controlling movements of the ego vehicle based on the updated trajectory, wherein the prediction of a trajectory of one of the surrounding vehicles comprises considering the ego vehicle's influence on a longitudinal acceleration of said surrounding vehicle.

Description

SYSTEM AND METHOD FOR CONTROLLING AN AUTONOMOUS VEHICLE IN A MULTI-LANE DRIVING ENVIRONMENT TECHNICAL FIELD [0001] The present disclosure relates to the field of automatic vehicle control. In particular aspects, the disclosure relates to techniques for controlling an ego vehicle in a multi-lane driving environment where non-controlled surrounding vehicles are present, wherein the trajectories of the surrounding vehicles are predicted while considering the ego vehicle’s influence on the longitudinal acceleration of said surrounding vehicles. The teachings to be disclosed herein can be applied to heavy- duty vehicles, such as trucks, buses, and construction equipment, among other vehicle types. Although the description may refer to a particular vehicle or vehicle type, the applicability disclosure is not so restricted. BACKGROUND [0002] Considering the vast amount of existing traffic situations, trajectory planning in safety-critical traffic scenarios remains an active research topic. In this disclosure one such scenario will be considered: highway driving maneuvers in dense traffic, where inter-vehicle distances can be smaller than the vehicles length. These maneuvers depend on complex driver-to-driver interactions, which pose challenges as the possibility for communication between vehicles is limited in practice [1]. Most commonly, drivers utilize the turn signal and adjust the relative distances to show intention. Further, maneuvers become additionally challenging when including heavy vehicle combinations (HVCs), due to e.g., limited visibility, and larger vehicle dimensions [2] (see, e.g., a forced lane change in Fig.1). [0003] Navigating in traffic can be considered as path planning with moving obstacles. Prior work has addressed this problem with Model Predictive Control (MPC), grid/tree-searching methods, and learning-based methods, see [3] and sources therein for a detailed review. The MPC approaches [4]–[6] have received attention due to the ability to enforce rigorous constraints on the dynamics of the own vehicle (henceforth referred to as the ego vehicle) and safety with respect to avoiding collisions with other vehicles (henceforth referred to as surrounding vehicles regardless of their exact positions relative to the ego vehicle). In this context, MPC iteratively solves a constrained optimization problem based on current measurements of the environment and computes control actions for a finite time/space window, referred to as the horizon. However, without vehicle-to-vehicle communication, MPC requires a prediction of the surrounding traffic over the horizon [3]. Since humans (human drivers) do not abide by Newtonian laws, predicting their trajectories is non-trivial and especially challenging when considering crowded environments with many interpersonal dependencies. The prediction of such trajectories has recently been addressed by learning-based predictors [7], which may also account for interactions among surrounding vehicles. The predicted trajectories can then be used to formulate collision avoidance constraints for an MPC-based trajectory planner, showing promising results in simulations of dense traffic scenarios [5]. In such interactive scenarios, there exists an inherent dependence between the planned MPC trajectory and the predicted trajectories of other road users. Hence, by decoupling the predictor and the planner, the predictions become less accurate and the MPC is not able to account for inter- actions with other vehicles. In current practical applications, interactive lane change maneuvers for AVs can become excessively defensive or even be avoided [2], [8]. [0004] This is one of the problems to be addressed in the present disclosure. SUMMARY [0005] One objective of the present disclosure is to make available a vehicle control method which allows efficient navigation in a multilane driving environment. A further objective is to improve the ego vehicle’s ability to change lanes within this driving environment without bothering surrounding vehicles unnecessarily. A further objective is to make available such a vehicle control method which is as safe as comparable methods according to the state of the art. A still further objective is to provide a computer system and a computer program for this purpose. [0006] In a first aspect of the present disclosure, there is provided computer- implemented method of controlling an ego vehicle in a multi-lane driving environment, wherein the method comprises performing the following by means of processing circuitry in a computer system: sensing position data of one or more surrounding vehicles, which are included a multi-lane driving environment; planning a trajectory of the ego vehicle in the multi-lane driving environment; predicting trajectories of the surrounding vehicles for at least the duration of the trajectory of the ego vehicle, wherein the prediction is based on the sensed position data of the surrounding vehicles; updating, at least once, the trajectory of the ego vehicle in view of the predicted trajectories of the surrounding vehicles; and controlling movements of the ego vehicle based on the updated trajectory. According to said first aspect, the prediction of a trajectory of one of the surrounding vehicles comprises considering the ego vehicle’s influence on a longitudinal acceleration of said surrounding vehicle. [0007] In other words, the above-stated problem is addressed by coupling the predictor with the planning controller. As will be described in detail below, the ego vehicle’s influence can be simulated in said prediction based on a lateral closeness of the surrounding vehicle and/or based on the ego vehicle’s use of a turning indicator. [0008] The proposed method is applicable, in particular, to highway driving maneuvers in dense traffic, where inter-vehicle distances can be smaller than the vehicles length. [0009] The proposed method is applicable, in particular, to traffic without any vehicle-to-vehicle communication, whether between the ego vehicle and the surrounding vehicles or among the surrounding vehicles. [0010] In a second aspects, there is provided a computer system for controlling an ego vehicle in a multi-lane driving environment, substantially by performing the method of the first aspect. [0011] There is further provided a computer program containing instructions for causing a computer system to carry out the method of the first aspect. The computer program may be 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. [0012] In the present disclosure, a “vehicle” can refer to a vehicle combination. In particular, the ego vehicle may be a combination of two or more vehicle units, such as a heavy vehicle combination (HVC). [0013] 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 this is explicitly stated. [0014] The disclosed aspects, examples (including any preferred examples), and/or accompanying claims may be suitably combined with each other as would be apparent to anyone of ordinary skill in the art. Additional features and advantages are disclosed in the following description, claims, and drawings, and in part will be readily apparent therefrom to those skilled in the art or recognized by practicing the disclosure as described herein BRIEF DESCRIPTION OF THE DRAWINGS [0015] Aspects and embodiments are now described, by way of example, with reference to the accompanying drawings, on which: figure 1 is a top view of a multi-lane driving environment; figure 2A illustrates collision avoidance constraints for a lane-keeping controller; figure 2B illustrates collision avoidance constraints for a left-lane-change controller; figure 2C illustrates collision avoidance constraints for a right-lane-change controller; figure 3 is a top view of a generated force-lance-change scenario, wherein the top panel, middle panel and bottom panel refer to the 1^st, 5^th and 15^th iteration, respectively; figure 4 is a flowchart of a method of controlling an ego vehicle in a multi-lane driving environment according to embodiments herein; and figure 5 is a block diagram of a computer system. DETAILED DESCRIPTION I. Outline [0016] The detailed description set forth below provides information and examples of the disclosed technology with sufficient detail to enable those skilled in the art to practice the disclosure. [0017] There will be presented a novel vehicle control method based on Distributed Model Predictive Control (DMPC), and the possibilities of the method will be demonstrated with a highway-autopilot controller for an HVC that considers interactions. The proposed method can in principle be combined with any predictor, possibly non-differentiable, that can be conditioned on a given trajectory, such as that proposed in [7]. Hence, some important contributions of the present disclosure are: - a novel method for combining trajectory prediction and planning by utilizing DMPC; - a demonstration of this approach for a novel problem, employing non-linear MPC for an HVC in dense, and interaction-dependent traffic; - an empirical analysis of the effect of prediction errors on the MPC performance and the convergence of the proposed method. A simulation environment together with a model-based predictor were developed for the purpose of this disclosure. II. Introduction to distributed optimal control [0018] MPC has been used in a wide range of multi-agent scenarios to optimize performance metrics and ensure safety-critical constraints. One implementation of MPC is a centralized approach, which considers a single controller that optimizes the combined objective of all agents over the horizon with respect to all states ^ and control variables ^, most often subject to some constraints. In network control problems, agents with local objectives, local constraints and coupled inequality constraints are often encountered. Such problems can be solved centrally, but their computational complexity typically scales poorly with an increasing number of agents [9]. One approach to addressing this scaling behavior is to decompose the central problem using DMPC. The network control problem can instead be expressed for each agent ^ over a discrete horizon ^ = 0, … , ^ as ^^^ m ^ in ^_^,^^^_^ ⁽^⁾^ + ^ ^_^ ^^_^ ⁽^⁾, ^_{^ ^},^_^ ⁽^⁾^ ^^^ s.t. ^_^ ⁽^ + 1⁾ = ^_^ ^{^}^_^ ⁽^⁾, ^_^ ⁽^^{)^},  ^ = 0, … , ^ − 1 (₁) ℎ_^^^⁽^⁾, ^⁽^⁾^ ≤ 0,      ^ = 0, … , ^ − 1 ^_^ ⁽0⁾ = ^_^,^ where the objective functions ^_^,^ and ^_^ describe the target and running cost of each agent ^, respectively, ^_^ describes the dynamics, ℎ_^ describes the coupled constraints, ^_^ and ^_^ are states and control inputs, respectively, and ^_^,^ are initial states. The coupling among the agents is imposed by the inequality constraint ℎ_^, which is a function of the states and control inputs of all the agents. A solution for the central problem is then obtained by overhead communication between all ^ agents. Rawlings et al. [10] describes “Non-cooperative” DMPC, where communication is restricted to the solution of each agent’s respective optimization problem. In this setting, agents aim to minimize their own objective function, treating other agents’ predicted trajectories ⁽^_^, ^_^ ^{)^} as known disturbances. Each agent’s beliefs of the other agents’ trajectories can be updated using a convex combination of their current optimal solution (^_^, ^_^)^⋆ and the current iterate (^_^ , ^_^)^{^}, as (2) The step size represents each agent’s belief in their own current optimal solution where 0 < 1. In practice, the convex step is iterated until there is convergence within a certain tolerance. [0019] It is noted that for the problem under consideration, only the ego-vehicle can be controlled. The development of the trajectories of the surrounding vehicles are estimated by a predictor. Considering non-Newtonian human drivers, the learned control policies of the surrounding vehicles could be non-differentiable, or even obtained by black-box approaches, and closed-form expression for their ^_^, ^_^ or ℎ_^ functions may not be available. It is shown in the present disclosure, however, that it may still be possible to implement a DMPC strategy using a gradient-based method for the ego-vehicle controller. III. Vehicle modeling [0020] This section provides a model of the ego vehicle and the surrounding traffic used in simulations. A. Ego-vehicle model [0021] The ego-vehicle motion is modeled using the following kinematic model, which may be referred to as a kinematic bicycle model extended with a trailer: (₃ ⁾ Here, ^_^ are states that include x- and y-position at the tractor-trailer joint in the inertial frame ^_^ , ^_^, longitudinal velocity at the tractor-trailer joint in the inertial frame ^_^ and the tractor and trailer angles with respect to the inertial frame ^_^. The vehicle states are controlled by the steering angle ^ and the longitudinal acceleration ^_^ at the tractor-trailer joint expressed in the body frame. The wheelbase of the tractor and trailer are represented by ℓ_^, ℓ_^, respectively. [0022] For purposes of simulation and optimization on a computer system, the model (3) may be discretized. The resulting discrete-time dynamic model can be written as ^_^(^ B. Traffic model [0023] Figure 1 is a top view of an example multi-lane driving environment 100 including a plurality of parallel lanes 101, 102, 103 and a highway exit (e.g., exit ramp) 104. The ego vehicle, here an HVC, carries the reference number 110, and the surrounding vehicles are indicated by reference number 120. Lane changes between adjacent pairs of the parallel lanes 101, 102, 103 is permitted, as is a change from the right lane 103 to the exit ramp 104. Such changes can be performed by the ego vehicle provided that the nearby surrounding vehicles facilitate this, e.g., by an accelerating or decelerating action aiming to accommodate the ego vehicle in the new lane. This behavior has hitherto not been adequately modeled for vehicle-simulation and vehicle-control purposes. [0024] Each of the surrounding vehicles is modeled using a kinematic bicycle model. The state ^_^ and control vector ^_^ of a vehicle ^ are described as for the ego- vehicle but excluding the trailer state ^_^. The longitudinal acceleration of each vehicle ^_^ is calculated based on tracking its own reference velocity and maintaining a safe distance to leading vehicles in the same lane. Additionally, vehicles can consider accelerating or decelerating to allow a vehicle in an adjacent lane to merge into their own, the extent of which is determined by how cooperative the vehicle is [11]. Similar to equation (4), such a traffic model can be described in time-discretized form by means of a general function, as ^_^ ⁽ _{^ + 1} ⁾ _{= ^^} ⁽ _^ ⁽ _^ ⁾ _{, ^^} ⁽ _^ ⁾ _{, ^} ⁾ _, ⁽ ₅ ⁾ where ^ are model parameters and ^ are the states of all surrounding vehicles. The steering angle ^ of surrounding vehicles has been considered zero in the studied traffic scenarios, as focus is placed on whether surrounding vehicles can yield a sufficient gap for the ego vehicle to fit into by accelerating or decelerating. However, it is straightforward to apply the proposed method to scenarios where surrounding vehicle may also change lanes. IV. Ego-vehicle problem formulation [0025] The trajectory planning problem, even for a single vehicle, is a mixed- integer program due to the necessity of choosing a target lane, and this is inherently an integer decision. Instead of solving a single mixed-integer problem, it was proposed in [4], [6] to solve three MPCs, each with a different target lane, i.e., keep current lane (no lane change), change to the left lane, and change to the right lane, denoted by ^ ∈ {nc, lc, rc}, respectively. A decision manager compares the costs of the three optimal MPC solutions and returns the safe optimal choice, e.g., driving forward, and a desired optimal choice, e.g., a forced lane change. [0026] Compared to [4], [6], there will additionally be considered a scenario with blocking traffic where the ego-vehicle is not able to immediately initiate a lane change, but it instead cautiously approaches adjacent vehicles to gauge their cooperativeness. A. Collision avoidance constraints [0027] As the collision avoidance constraints are based on estimations of trajectories ^{^}^ ^ ^ ^ , ^^^{^} of surrounding vehicle ^ for each controller ^, deviations from the true trajectories can yield infeasible problems in edge cases. Hence, slack variables ^^× ^ ∈ ℝ ^{^^} are introduced to relax the collision avoidance constraints, where represents a subset of the surrounding vehicles, with size ^ ^ ^ . These receive a substantial penalty in the objective to remove the possibility of collisions in a practical sense but persistent feasibility is no longer rigorously established. [0028] For the lane keeping controller ^ = nc, avoiding collisions amounts to maintaining a longitudinal distance to a leading vehicle ^, while remaining in the current lane. The longitudinal distance has to satisfy where ^_^ represents a distance margin, ℓ_^ is the tractor length, ^_^ represents the time- headway and ^^^ ^_,^ is the slack variable. The position ^̂^^ ^_,^ of a car is defined with reference to the center of its rear side, and the position of the HVC is defined reference to the center of the trailer’s rear side; hence, ^^{^^} corresponds to center of the front side of the trailer in the HVC. The constraint (6) thus defines a minimum allowed longitudinal distance. To avoid collisions with vehicles in adjacent lanes, the lateral position is constrained to the current lane margins, accounting for the maximum lateral span of the ego-vehicle ^_^,^. A visualization of the constraints with the referenced parameters is displayed in Fig.2A. [0029] In the lane changing case, ^ ∈ {lc, rc}, both the longitudinal and lateral distance to some vehicle ^ are constrained. This yields a possibly non-differentiable constraint which can be formulated as a mixed integer problem (MIP). However, as MIPs are significantly more computationally intensive to solve, a smooth approximation ^^ ^ ^ of the constraint boundaries using a sum of tanh functions will be employed, more precisely where ^ ∈ {lc, rc}. Here, ^ ^ ^,^ and ^ ^ ^,^ scale the constraint in lateral and longitudinal distance, respectively, and incorporate the surrounding vehicle position, width and length, together with those of the ego-vehicle, plus a safety margin. Lastly, ^ ^ ^,^ shifts the constraint in lateral distance based on the current lateral position of the surrounding and ego-vehicle. A visualization of the constraints for both left and right lane changes are displayed in Fig.2B and Fig.2C. Hence, the collision avoidance constraints can be formulated as where ^ ^ ^ ∈ {−1,1} converts between an upper and lower constraint on lateral position, depending on the vehicle ^ and controller ^. This constraint is repeated for all surrounding vehicles ^ ∈ ^ ^ ^ . [0030] Finally, constraints (6) and (8) together with the physical limitations on the ego-vehicle states and control input form the general constraints for each controller ^, where ^^{^(}^⁾ and ^^{^(}^⁾ gather the estimated/predicted states and control inputs of all surrounding vehicles. B. Optimal control problem formulation [0031] Each MPC ^ of the ego-vehicle aims to track a reference in longitudinal velocity and lateral position while minimizing the control input. The objective also addresses driver comfort by minimizing the change in acceleration and steering angle. Expressing the objective over the horizon for each controller ^, including slack variables, yields ^ ^ ^ ^ ^ ^ ^ ^ ^_^ , ^_^ , ^_^ =   ^_^ ^{( )} _^,^ ^{( )} _^ ^ ^ + ^ ^ ^ ^ ^ ^ _{^ − ^ ^} ^ _^ ⁽ _^ ⁾ _^^_^ ^^^ + ^^{^}^ ^ ^ ^ ^ ^⁽ _^ ⁾ _{− ^^,^} (_^)^^ + ^{^}^ ^ ^(_^)^^_^ ^^^ ^^^ ^ (10) + ^^^ ^ ^⁽ _^ ⁾ _{− ^} ^ ^_,^ ⁽ _^ ⁾ _^ ^ ^^^ ^^^ ^ + ^^^ ^ ^(^ + 1) − ^ ^ ^_,^ (^)^^_^ ^^^ where ^_^ = ^_^^_^^ , ^ = diag⁽^_^ , ^_^ ⁾, ^_^ = diag⁽^_^^ , ^_^^ ⁾ and ^ = diag^0, ^_^ , ^_^ , 0,0^ are positive semi-definite matrices and ^ ^ ^ ^_,^ = ^0, ^ ^ ^_,^ , ^_^,^, 0,0^ , ^ ^ ^_,^ = ^[0,0^{]^} represent the state and input reference for each controller. The ego vehicle states over the horizon are gathered in ^ ^ ^ = ^^ ^ ^(0), ^ ^ ^(1), … , ^ ^ ^(^)^,  ^ ∈ {nc, lc, rc} ⁽11⁾ and similarly are the control inputs and slack variables in ^ ^ ^ and ^ ^ ^ . The positive definite terminal cost matrix ^ is determined using an infinite horizon linear quadratic regulator (LQR) by solving the discrete time Riccati differential equation for linearized dynamics (3) around ^_^,^ and ^_^,^; see [10] for more details. [0032] It is now possible to express the optimal control problem for the ego- vehicle and each respective controller ^ ∈ {nc, lc, rc}, namely, ℎ ^ ^ ^^ ^ ^⁽ _^ ⁾ _{, ^} ^ ^⁽ _^ ⁾ _{, ^} ^ ^⁽^⁾, ^^{^(}^⁾, ^^{^(}^⁾^ ≤ 0, ^ ^ ^ ≤ 0; ^_^ ⁽0⁾ = ^_^,^; ^^^{^} , ^^{^}^ = ^^⋅, ^ ^ ^ , ^ ^ ^^, where the constraints in (12) are imposed ∀^ = 0, … , ^ − 1 and ^^^{^} , ^^{^}^ are estimated/predicted trajectories of surrounding vehicles over the horizon, gathering ^^{^(}^⁾, ^^{^(}^⁾ for all ^ like in (11). The predictor ^ will be described in more detail in below Section V. It is important to note here that sensitivities (derivatives) of the predictor ^ may not be available and it may not be possible to directly cast problem (12) in the form of a standard nonlinear program (NLP). Indeed, one aim of the present disclosure is to decouple the predictor ^^^{^} , ^^{^}^ = ^^⋅, ^ ^ ^ ^ , ^_^^ such that the subproblem in (12) can be cast in the form of a standard NLP. [0033] After each of the three MPCs are finished computing, the decision manager selects the control signals from the MPC that optimizes where ^ ^ ^, ^_^, ^_^ are positive scaling parameters and ^_^ is current cost of the ^^th MPC. The function ^ ^ ^ introduces a cost on switching ^ based on ^ previous decisions, as where ^ is the history of previous choices of ^ and I is an indicator function. The function ^ ^ ^ introduces a cost on the distance to a highway exit 104 as where ^_^^^^ is the distance to the exit, ^_^^^ > ^_^^^^, ^ ∈ ^[0,1^] are considered tuning parameters and ^^⋆ is the decision that guides the ego vehicle towards the highway exit [6]. V. Predicting and planning using DMPC A. Main algorithm [0034] As outlined in Section IV, a trajectory planner in highway traffic can be designed using MPC with local objectives and constraints, combined with coupled inequality constraints. Without vehicle-to-vehicle communication, the coupled constraints require a prediction of the surrounding vehicles trajectories, which in turn depend on the planned ego-vehicle trajectories. In the example, a general predictor ^ as described in [7], which returns trajectories ^^, ^^ over a future horizon, will be considered. The predictor utilizes past observations of all vehicles ^_^^^, ^_^^^ together with the planned trajectory of the ego-vehicle ^_^, ^_^ over the future horizon. This may be written as ^^, ^^ = ^(^_^^^, ^_^^^, ^_^, ^_^). (16) [0035] In the DMPC formalism, the predicted variables can be treated as an approximation of the local solution of each agent’s optimal control problem (1). Hence, if the prediction ^^^{^}, ^^{^}^ at some iterate ^ can be used to express the coupled constraints, it is possible to apply the method presented in Section II to formulate an iterative algorithm that attempts to solve the centralized problem by distributing the ego-vehicle planner from its predictor. Given a fixed prediction ^^^{^}, ^^{^}^, problem (12) becomes a smooth NLP: where the index ^ will henceforth be omitted for convenience. Predicted and planned trajectories that remain constant over iterates ^ indicate that both concur. It is noted that the convergence, which has been proved for linear systems in [10], cannot be generalized to this problem as a non-linear MPC with prediction errors and a general predictor is now being considered. To prevent divergence, a loss metric ^, which can treat prediction and planning jointly, will be applied. For the ^ + 1 iteration, the loss metric is given by The approach followed in this disclosure forces the loss metric to decrease at each iteration, i.e., ^^{^^^} < ^^{^}. If the loss metric stops decreasing, or if it drops below a preconfigured tolerance ^, the iteration is terminated. This trivially means that the optimization cannot diverge from the closest local solution of the trajectory initialization. [0036] The following Algorithm 1 displays the formulation of the coupled predicting and planning method, employing the DMPC scheme for (17). Algorithm 1: Predicting and planning using DMPC 1 for ^ = 1, … , ^_^^^ do Initialize: ^^^{^} _^,^ , ^_^ ^{^} _,^ ^ = shift^^ ⋆ ^_,^^^ , ^_^ ^⋆ ,_^^^ ^ ^^^{^} _^ , ^_^ ^{^}^ = ^^^_^^^,^, ^_^^^,^, ^^{^} _^,^ , ^_^ ^{^} _,^ ^ ^^{^} = ∞ for ^ = 0, … , ^_^^^ do Plan: ⋆ ^_^ ^^^ ^_,^ , ^ ^^^ ^_,^^ = ^ ^ ^ ^_^^ ^{^}^_^ , ^_^^ Update: ^^ ⋆ ^ ^ ← ^^^ ^^^^ + 1 − ^ ^_{,^ ^ ^,^} ⁽ ^_^ ⁾^_^,^ ^^^ ^^ ⋆ ^ ← ^ ^ ^^ ^ ^_{,^ ^} ^{^} _^,^ + ⁽1 − ^_^ ⁾^_^,^ Predict: ^^ ^ ^, ^_^^ = ^^^ ^^ ^^^ ^_^^,^, ^_^^^,^ , ^_^,^ , ^_^,^^ Update: ^ ^^^ ← ^^ + ^ ^ _^ ⁽1 − ^⁾^_^ ^ ^^^ ^ ^ ← ^^_^ + ⁽1 − ^⁾^_^ Compute: ^^{^^^} using (18) if ^^{^} < ^^{^^^} then (_^ ^⋆ _{^,^ ^} ^⋆ ^ ^ ⋆ , ^_,^ ) = ^^_^,^ , ^_^,^ ^ , exit the loop else if ^^{^^^} < ^ then (_^ ^⋆ _^,^ , ^_^ ^⋆ _,^ ) = ^^ ^^^ ^_,^ , ^ ^^^ ⋆ ^_,^^ , exit the loop end if 14 end for 1⁵ Apply the first control action ^_^ ⁽0|^⁾ from ^_^⋆ ,_^ 16 end for In a proper MPC setting, an index ^ is introduced when MPC is updated, such that each variable, e.g., the ego vehicle state, can be written as ^_^ ⁽^ + ^|^⁾, or even simpler as ^_^ ⁽^|^⁾. The predicted trajectories over the horizon, e.g., the states as in (11), can be written as ^ ^ ^_,^ . In line 4, the algorithm computes an optimal MPC solution, denoted as ^ ^ ⋆ ^ ^^ ^_,^ , ^ ^^^ ^_,^ ^ , by iterating the DMPC method over ^ towards ^_^^^ at each time step ^ until ^_^^^. Notice that the ego trajectories updated in line 5 are used only for computing the loss metric and by the predictor, while the returned DMPC solution ^^ ^∗ _^,^ , is from one of the iterates in line 4, i.e., before performing the update. Finally, the function shift⁽⋅⁾ shifts the optimal variables from time step ^ − 1 and pads the last value using zero control [10]. The shift function is used to derive an initial guess for variables at time step ^. B. Interactive lane changes [0037] As a basis for an example predictor ^, the inventors have developed an extended model, referred to as I-IDM, for capturing two main modes of driver-to- driver interaction: relative distance and use of turning indicators. [0038] Similar to the approach in [5], it is here proposed to use a cooperation ℓ coefficient ^_^ ∈ ^ _^ ^ , ℓ_^^, where ℓ_^ is the width of one lane 101, 102, 103. When interacting with HVCs, cooperative surrounding vehicles tend to both accelerate and decelerate, as needed depending on the relative longitudinal position to the HVC [2]. Different to [5], the I-IDM will apply a forward difference ℓ_^ and a rear distance ℓ_^ to define metric sets (or point sets) ^ ^ , ^^ , wherein ^ ^ _^ ^^ is a rectangle that extends ℓ_^ units ahead of and ^_^ units to the side of a reference point on the vehicle ^, and wherein ^^ ^ is a rectangle that extends ℓ_^ units rear of and ^_^ units to the side of the reference point. The reference point may be the center of the rear side of the vehicle. The following conditions for identifying an HVC as a leading or following vehicle in the I-IDM model are defined, also addressing collision avoidance with other vehicles. Definition 1: An HVC is a leading vehicle if ^^_^,^, ^_^,^^ ∈ ^ ^ ^ and ∀^ ^^_^,^, ^_^,^^ ∉ Here, (^_^,^, ^_^,^) represents a position of the HVC (cf. equation (1)), and ^^_^,^ ∈ {0,1} represents the HVC turn signal indicator in the direction of the vehicle ^, ^ is an index of a vehicle in the multi-lane driving environment 100, and ^ is the index of the HVC. A turning indicator may alternatively be referred to as a turn-signal indicator or as a direction-signal indicator in the sense of [14]. [0039] Alternatively, the I-IDM can be implemented with ^^_^,^ absent from Definition 1 or Definition 2, or both of these. [0040] Due to the large length difference between an HVC and a car, the desired relative lateral distance can be vastly different from the desired relative longitudinal distance. Hence, instead of considering the Euclidean distance between the point mass models, it is proposed to represent the distance ^ in the IDM as the smallest relative distance between the point mass of vehicle ^ and the HVC frame. As the shape of the HVC has a complex geometry, it is proposed to approximate the distance to obtain a computationally efficient solution, by modeling the tractor and trailer by their longitudinally extending center lines, denoted ^_^^^_^,^ , ^_^,^, ^_^,^^ and ^_^^^_^,^, ^_^,^, ^_^,^^. The shortest distance can be approximated by projecting the position (^_^,^ , ^_^,^) of the surrounding vehicle orthogonally onto ^_^ ^{^}^_^,^ , ^_^,^, ^_^,^ ^{^} and ^_^^^_^,^, ^_^,^, ^_^,^^, and by mapping said points on the intersection of the lines and horizontal surfaces ^_^, ^_^ representing the footprint of the tractor and trailer. Formally, the shortest distance (minimum lateral separation) is given by 1,2 where ^: ^ ↦ ^_^ ∩ ^_^ and proj_^(^) denotes an orthogonal projection of a point ^ ∈ ℝ^ on a line ^. With this notation, governing equations for the I-IDM can be written: with where ^ = 1 if the HVC is identified as a leading vehicle and ^ = −1 if the HVC is identified as a following vehicle in the sense of the above Definitions 1 and 2 or their alternative formulations. In the present disclosure, the model parameters are tuned to avoid dangerous deceleration profiles using the definition ^_^^^^ in [15]. An accurate tuning of the model parameters can be made based on data from real-world scenarios. This will be within the abilities of those skilled in the art. VI. Simulation study [0041] This section presents a case study with the proposed methods, investigating performance and convergence with respect to prediction errors. A. Simulation setup [0042] A simulation environment [13] controls the surrounding vehicles using the models described in Section III.B, wherein the ego-vehicle is controlled using the disclosed MPC. The traffic model parameters are randomized using uniform distributions as The MPC problem is formulated using Casadi, running the IpOpt solver [12]. The approach presented in Section V is demonstrated using a model-based predictor ^, utilizing the model in Section III.B with added noise ^(0, ^_^) to the control inputs of the surrounding vehicles. The predictor hence utilizes a measurement of surrounding vehicles, repeatedly applies the ground-truth model using the planned ego-vehicle trajectory and adds normal distributed noise to introduce a prediction error that can be scaled with ^_^. Note that this predictor may perform less well in estimating and predicting the motion of real vehicles, which may require learning-based methods. Yet, this approach is deemed suitable to clearly demonstrate the proposed DMPC algorithm and investigate the impact of prediction errors. B. Test scenarios [0043] The dense traffic environment is simulated by randomly sampling ^ vehicles 120 in close proximity of the ego-vehicle 110 located in the middle lane 102. Longitudinal inter-vehicle distances in adjacent lanes are kept smaller than the length of the ego-vehicle. The simulations are initialized such that a lane change is feasible, if properly handled by the controller. Fig.3 illustrates a sampled scenario. In this setting, a “Forced lane change” maneuver (FLC) is considered where the ego- vehicle 110 is required to reach a highway exit at ^_^,^^^^ = 250 m, ahead in the rightmost lane 103. The desired speed ^_^,^ is 60 km/h and a lane change is defined as successful if the right lane’s center ^_^ ^{^^} _,^ is reached before the exit. The maximum considered time frame is ^_^^^ = 30 s. Two separate controller architectures for the ego-vehicle are studied: 1.) “DC-MPC”: The ego-vehicle considers decoupled prediction and planning; 2.) “PP-DMPC” The ego-vehicle couples prediction and planning using Algorithm 1. The PP-DMPC hyperparameters are chosen as: ^_^^^ = 15, ^ = ^_^ = 1/⁽^ + 1⁾ and ^ = 5. Hence, both controllers utilize the same predictive capabilities of ^ and gauge interactions with other vehicles by first approaching the lane markers of the desired lane. The PP-DMPC controller further considers how the ego-vehicle trajectory interacts with the predicted trajectory of the surrounding vehicles through Algorithm 1. Simulations are evaluated with three different noise settings for ^: ^_^,^ = 0.1, ^_^,^ = 1, and ^_^,^ = 2. It is noted that the acceleration of each vehicle is confined within a defined physical limitation of ±4 m/s², both in simulation and in the predictor. C. MPC performance results [0044] Qualitative analysis. Fig.3 highlights the key characteristics of the coupled trajectory prediction and planning according to the proposed method. The top plot displays a naive, but feasible initialization of the state trajectories for the ego- vehicle and an adjacent vehicle. As iterations in the distributed scheme continue, the predictor and planner reason via coupled variables. The middle plot displays emerging interactions between the planned ego-vehicle trajectory (diamonds with forward hashing //) and an adjacent vehicle (diamonds with reverse hashing \\) at ^ = 5. Finally, the bottom plot displays a converged PP-DMPC solution at ^ = 15, where the adjacent vehicle in the right lane has slowed down to allow the ego vehicle to make a lane change. [0045] Quantitative analysis. To evaluate the performance of both controllers, 100 scenarios were sampled. The inventors evaluated the overall performance with respect to the following aspect: - Success Rate: The percentage of scenarios that achieve the goal of the respective case; - Collision Rate: The percentage of scenarios resulting in a collision; - Time-Out Rate: The percentage of scenarios that fail to complete the case objective within the time/space frame. The inventors furthered considered treated an optimality aspect by considering: - Total Cost: An evaluation of the total objective function (10). The total cost is expressed as a percentage of the objective function evaluated for PP-DMPC for ^_^,^. These metrics are presented in Table 1, where results for different predictors ^_^,^/^_^,^/^_^,^ are separated by slash signs. Controller DC-MPC PP-DMPC Success (%) 98/96/88 100/92/91 Collision (%) 0/0/0 0/0/0 Time-Out (%) 2/4/12 0/8/9 Total cost (%) 95.0/95.2/99.2 93.1/99.4/100.0 Average number of – 4.14/3.01/2.27 iterations (^) Convergence Rate – 83.4/18.1/4.8 [0046] For the low-noise predictor ^_^,^, the PP-DMPC outperforms the DC-MPC, obtaining a higher success-rate and lower total cost. The performance of both the DC- MPC and the PP-DMPC decreases with an increasing ^, and the PP-DMPC does not consistently out-perform the MPC. This indicates that Algorithm 1, applied for the proposed non-linear MPC can have benefits, however also that this is reliant on the accuracy of the predictor used. D. Convergence and prediction errors [0047] The average number of iterations for the PP-DMPC and the convergence rate for each predictor is displayed in Table 1. This indicates that the convergence of Algorithm 1 is dependent on the accuracy of the predictor. An analysis of the distribution of the loss ^ and its gradient ^^{^^^} − ^^{^} reveals that, on average, larger prediction errors result in a larger loss and larger corresponding gradients. The loss gradients are indeed never positive, which empirically confirms that the algorithm does not diverge from the closest local minima. VII. Implementations [0048] Attention will now be directed to implementations of this framework in a utility context. With reference to the flowchart in figure 4, a method of controlling an ego vehicle 110 in a multi-lane driving environment 100 (see Fig.1) according to various embodiments will now be described. [0049] In some embodiments, the method begins with an optional step 410 of receiving a lane-change instruction, which is to be considered in the planning of the trajectory of the ego vehicle. The lane-change instruction may be one of nc, lc, rc, and its validity period may either be specified together with the instruction or be preconfigured. The lane-change instruction may be carried out by up-weighting or down-weighting the respective nc, lc, rc costs applied by the decision manager that was discussed in Section IV, which influences the “desired optimal choice” and possibly the “safe optimal choice” as well. [0050] In a second step 412, position data ^_^,^ of one or more surrounding vehicles 120 is sensed. The surrounding vehicles 120 are included the multi-lane driving environment 100. Although the position data ^_^,^ of the ego vehicle 110 may be read as an internal state variable maintained by the entity performing the present method or using an ego-state observer, it is understood that this entity may not be authorized to directly control movements of the surrounding vehicles 120 and may not have access to their position data. Instead, sensors may be relied upon to fetch the position data ^_^,^, especially environment sensors in the traffic scene. The position data may include one or more of: position (e.g., geo-position), pose (e.g., orientation), linear speed, angular speed, linear acceleration, angular acceleration. [0051] In a third step 414 of the method, a trajectory of the ego vehicle 110 in the multi-lane driving environment 100 is planned. The trajectory planning may comprise executing a model-predictive control (MPC) algorithm. The trajectory planning may comprise solving the problem (12). A result of the third step 414 may include a position sequence and a control-signal sequence, [0052] In a fourth step 416, trajectories ^ of the surrounding vehicles for at least the duration of the trajectory ^^ ^ ^ , ^ ^ ^^ of the ego vehicle are predicted. In line with equation (16), a general predictor ^ may be utilized which considers inter alia the sensed position data ^_^,^ of the surrounding vehicles and the trajectory ^^ ^ ^ ^ , ^_^^ of the ego vehicle determined in step 414. The predictor may include a model-based simulator, a distributed MPC algorithm, and/or a machine-learning predictor (trained model). Optionally, as a way to ensure that the trajectories of the surrounding vehicles are feasible, the step 416 may further attempt to determine control signals ^ for the surrounding vehicles – the trajectories become ^^, ^^ – even though the entity performing the present method need not be authorized to apply such a control signal to the surrounding vehicles. If, for some combination of sensed position data ^_^,^ of the surrounding vehicles and the trajectory ^^ ^ ^ ^ , ^_^^ of the ego vehicle, it turns out to be impossible to determine a control signal ^ that realizes the position sequence ^ for all of the surrounding vehicles, then this could be take as a sign of non-feasibility; the position sequence ^ may not be suitable for use in the subsequent steps of the method 400, and instead the prediction step 416 may be rerun. [0053] In particular, the step 416 of predicting a trajectory for a surrounding vehicle 120 may include a substep 416.2 of considering the ego vehicle’s 110 influence on a longitudinal acceleration of the surrounding vehicle 120. In some embodiments, the ego vehicle’s 110 influence is simulated based on a lateral closeness of the surrounding vehicle (see figure 2) and optionally taking into account the ego vehicle’s use of a turning indicator. [0054] The lateral closeness may correspond to a minimum lateral separation ^̂ of the ego vehicle and the surrounding vehicle in the lateral direction of the ego vehicle; see Section V.B. In particular, when the ego vehicle is a combination of two or more vehicle units, the lateral closeness is determined by minimizing, over all vehicle units, a minimum lateral separation ^̂ of the respective vehicle unit and the surrounding vehicle in the lateral direction of the vehicle unit. For each of these options, the minimum lateral separation ^̂ may be determined from a point on the ego vehicle’s body which is such that the lateral distance to the surrounding vehicle is minimized. The simulated influence of the ego vehicle on the longitudinal acceleration of the surrounding vehicle may further be based on a speed of the surrounding vehicle, a speed difference relative to the ego vehicle, and the minimum lateral separation. In particular, the simulated longitudinal acceleration of the surrounding vehicle may be inversely proportional to the square of the minimum lateral separation ^̂, as per the expression Section V.B. [0055] In some embodiments, the prediction of a trajectory of one of the surrounding vehicles in substep 416.2 further includes considering the ego vehicle’s influence on a lane-change behavior of said surrounding vehicle. For example, if the ego vehicle approaches the surrounding vehicle fast, the driver of the latter vehicle may want to make an avoidance maneuver to an adjacent lane. [0056] In a next step 418 of the method, the trajectory of the ego vehicle 110 is updated at least once in view of the predicted trajectories ^ of the surrounding vehicles 120. The updating 418 may be carried out similarly to steps 414 and 416, though for different initial values. It is assessed in a step 420 whether the trajectory of the ego vehicle 110 is satisfactory (N branch) or whether further updating is deemed necessary (Y branch). In some embodiments of the method, the assessment may include evaluating whether the loss gradient ^^{^^^} − ^^{^} has fallen below a threshold ^ or whether the loss ^^{^^^} is less than such a threshold ^; cf. line 11 of Algorithm 1. In other embodiments, especially if the method is performed in real time and hence under time constraints, a preconfigured number of updates 418 may be performed. [0057] The updating step 418 may include a substep 418.2 of re-planning the trajectory of the ego vehicle in the multi-lane driving environment, in which the surrounding vehicles move according to the predicted trajectories, followed by a substep 418.4 of interpolating between the existing trajectory and the re-planned trajectory of the ego vehicle. The interpolation may be in accordance with a convex combination, as per equation (2) or line 7 of Algorithm 1. [0058] In a final step 422 of the method, the movements of the ego vehicle 110 are controlled based on the updated trajectory. Step 422 can be carried out using operational controllers in, or associated with, the ego vehicle. For example, the control signal sequence ^ ^ ^ for the chosen option among ^ ∈ {lc, rc, nc} may be transmitted to the ego vehicle 110 for execution. [0059] After step 422 has been initiated, all or some steps of the method may be repeated to support onward navigation, e.g., after a preconfigured time has elapsed or after the ego vehicle 110 has moved a preconfigured distance. [0060] The method according to figure 4 can be implemented by of a vehicle- carried or stationary computer system. The computer system may be communicatively connected to sensorics, actuators and communication means, as necessary in view of the steps of the relevant embodiment of the method. In particular, the method may be executed in real time during operation of the ego vehicle, i.e., while the ego vehicle 110 moves in the multi-lane driving environment 100. Further, it is possible to execute the method by an entity not authorized to directly control movements of the surrounding vehicles 120, or by an entity that should not or should preferably not make use of its authority to directly control movements of the surrounding vehicles 120. [0061] By way of example, figure 5 is a schematic diagram of a computer system 500 suitable for carrying out the method according to figure 4. [0062] The computer system 500 is adapted to execute instructions from a computer-readable medium to perform these and/or any of the functions or processing described herein. The computer system 500 may be connected (e.g., networked) to other machines in a LAN, an intranet, an extranet, or the Internet. While only a single device is illustrated, the computer system 500 may include any collection of devices that individually or jointly execute a set (or multiple sets) of instructions to perform any one or more of the methodologies discussed herein. Accordingly, any reference in the disclosure and/or claims to a computer system, computing system, computer device, computing device, control system, control unit, electronic control unit (ECU), processor device, processing circuitry, etc., includes reference to one or more such devices to individually or jointly execute a set (or multiple sets) of instructions to perform any one or more of the methodologies discussed herein. For example, control system may include a single control unit or a plurality of control units connected or otherwise communicatively coupled to each other, such that any performed function may be distributed between the control units as desired. Further, such devices may communicate with each other or other devices by various system architectures, such as directly or via a Controller Area Network (CAN) bus, etc. [0063] The computer system 500 may comprise at least one computing device or electronic device capable of including firmware, hardware, and/or executing software instructions to implement the functionality described herein. The computer system 500 may include processing circuitry 502 (e.g., processing circuitry including one or more processor devices or control units), a memory 504, and a system bus 506. The computer system 500 may include at least one computing device having the processing circuitry 502. The system bus 506 provides an interface for system components including, but not limited to, the memory 504 and the processing circuitry 502. The processing circuitry 502 may include any number of hardware components for conducting data or signal processing or for executing computer code stored in memory 504. The processing circuitry 502 may, for example, include a general-purpose processor, an application specific processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA), a circuit containing processing components, a group of distributed processing components, a group of distributed computers configured for processing, or other programmable logic device, discrete gate or transistor logic, discrete hardware components, or any combination thereof designed to perform the functions described herein. The processing circuitry 502 may further include computer executable code that controls operation of the programmable device. [0064] The system bus 506 may be any of several types of bus structures that may further interconnect to a memory bus (with or without a memory controller), a peripheral bus, and/or a local bus using any of a variety of bus architectures. The memory 504 may be one or more devices for storing data and/or computer code for completing or facilitating methods described herein. The memory 504 may include database components, object code components, script components, or other types of information structure for supporting the various activities herein. Any distributed or local memory device may be utilized with the systems and methods of this description. The memory 504 may be communicably connected to the processing circuitry 502 (e.g., via a circuit or any other wired, wireless, or network connection) and may include computer code for executing one or more processes described herein. The memory 504 may include non-volatile memory 508 (e.g., read-only memory (ROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), etc.), and volatile memory 510 (e.g., random-access memory (RAM)), or any other medium which can be used to carry or store desired program code in the form of machine-executable instructions or data structures and which can be accessed by a computer or other machine with processing circuitry 502. A basic input/output system (BIOS) 512 may be stored in the non-volatile memory 508 and can include the basic routines that help to transfer information between elements within the computer system 500. [0065] The computer system 500 may further include or be coupled to a non- transitory computer-readable storage medium such as the storage device 514, which may comprise, for example, an internal or external hard disk drive (HDD) (e.g., enhanced integrated drive electronics (EIDE) or serial advanced technology attachment (SATA)), HDD (e.g., EIDE or SATA) for storage, flash memory, or the like. The storage device 514 and other drives associated with computer-readable media and computer-usable media may provide non-volatile storage of data, data structures, computer-executable instructions, and the like. [0066] Computer-code which is hard or soft coded may be provided in the form of one or more modules. The module(s) can be implemented as software and/or hard- coded in circuitry to implement the functionality described herein in whole or in part. The modules may be stored in the storage device 514 and/or in the volatile memory 510, which may include an operating system 516 and/or one or more program modules 518. All or a portion of the examples disclosed herein may be implemented as a computer program 520 stored on a transitory or non-transitory computer-usable or computer-readable storage medium (e.g., single medium or multiple media), such as the storage device 514, which includes complex programming instructions (e.g., complex computer-readable program code) to cause the processing circuitry 502 to carry out actions described herein. Thus, the computer-readable program code of the computer program 520 can comprise software instructions for implementing the functionality of the examples described herein when executed by the processing circuitry 502. In some examples, the storage device 514 may be a computer program product (e.g., readable storage medium) storing the computer program 520 thereon, where at least a portion of a computer program 520 may be loadable (e.g., into a processor) for implementing the functionality of the examples described herein when executed by the processing circuitry 502. The processing circuitry 502 may serve as a controller or control system for the computer system 500 that is to implement the functionality described herein. [0067] The computer system 500 may include an input device interface 522 configured to receive input and selections to be communicated to the computer system 500 when executing instructions, such as from a keyboard, mouse, touch- sensitive surface, etc. Such input devices may be connected to the processing circuitry 502 through the input device interface 522 coupled to the system bus 506 but can be connected through other interfaces, such as a parallel port, an Institute of Electrical and Electronic Engineers (IEEE) 1394 serial port, a Universal Serial Bus (USB) port, an IR interface, and the like. The computer system 500 may include an output device interface 524 configured to forward output, such as to a display, a video display unit (e.g., a liquid crystal display (LCD) or a cathode ray tube (CRT)). The computer system 500 may include a communications interface 526 suitable for communicating with a network as appropriate or desired. [0068] The operational actions described in any of the exemplary aspects herein are described to provide examples and discussion. The actions may be performed by hardware components, may be embodied in machine-executable instructions to cause a processor to perform the actions, or may be performed by a combination of hardware and software. Although a specific order of method actions may be shown or described, the order of the actions may differ. In addition, two or more actions may be performed concurrently or with partial concurrence. [0069] To summarize, the present disclosure is concerned with optimal trajectory planning for an autonomous vehicle (AV) operating in dense traffic, where vehicles closely interact with each other. To tackle this problem, the inventors propose a novel framework that couples trajectory prediction and planning in multi-agent environments, using distributed model predictive control. A demonstration of this framework is presented in simulation, employing a trajectory planner using non- linear model predictive control. Performance and convergence are analyzed under different assumptions about the prediction errors. The results indicate, in broadly defined conditions, that the obtained locally optimal solutions are superior to the results of decoupled prediction and planning. [0070] The aspects of the present disclosure have mainly been described above with reference to a few embodiments. 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 1. P. Hidas, “Modelling vehicle interactions in microscopic simulation of merging and weaving,” Transportation Research Part C: Emerging Technologies, vol.13, no.1, pp.37–62, 2005. 2. P. Nilsson, L. Laine, and B. Jacobson, “A simulator study comparing characteristics of manual and automated driving during lane changes of long combination vehicles,” IEEE Transactions on Intelligent Transportation Systems, vol.18, no.9, pp.2514–2524, 2017. 3. S. Dixit, S. Fallah, U. Montanaro, M. Dianati, A. Stevens, F. Mccullough, and A. Mouzakitis, “Trajectory planning and tracking for autonomous overtaking: State-of-the-art and future prospects,” Annual Reviews in Control, vol.45, pp.76–86, 2018. 4. N. Murgovski and J. Sjöberg, “Predictive cruise control with autonomous overtaking,” in 201554th IEEE Conference on Decision and Control (CDC). IEEE, 2015, pp.644–649. S. Bae, D. Isele, A. Nakhaei, P. Xu, A. M. A˜non, C. Choi, K. Fujimura, and S. Moura, “Lane-change in dense traffic with model predictive control and neural networks,” IEEE Transactions on Control Systems Technology, 2022. J. Karlsson, N. Murgovski, and J. Sjöberg, “Optimal trajectory planning and decision making in lane change maneuvers near a highway exit,” in 201918th European Control Conference (ECC). IEEE, 2019, pp.3254–3260. T. Salzmann, B. Ivanovic, P. Chakravarty, and M. Pavone, “Trajectron++: Dynamically-feasible trajectory forecasting with heterogeneous data,” in Computer Vision–ECCV 2020: 16th European Conference, Glasgow, UK, August 23–28, 2020, Proceedings, Part XVIII 16. Springer, 2020, pp.683– 700. D. Sadigh, S. Sastry, S. A. Seshia, and A. D. Dragan, “Planning for autonomous cars that leverage effects on human actions.” in Robotics: Science and systems, vol.2. Ann Arbor, MI, USA, 2016, pp.1–9. F. Bertilsson, M. Gordon, J. Hansson, D. Möller, D. Söderberg, Z. Zhang, and K. Åkesson, “Centralized versus distributed nonlinear model predictive control for online robot fleet trajectory planning,” in 2022 IEEE 18th International Conference on Automation Science and Engineering (CASE). IEEE, 2022, pp.701–706. J. B. Rawlings, D. Q. Mayne, and M. Diehl, Model predictive control: theory, computation, and design. Nob Hill Publishing Madison, WI, 2017, vol.2, ch.6, pp.369–450. B. Brito, A. Agarwal, and J. Alonso-Mora, “Learning interaction-aware guidance for trajectory optimization in dense traffic scenarios,” IEEE Transactions on Intelligent Transportation Systems, vol.23, no.10, pp.18808–18821, 2022. J. A. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “Casadi: a software framework for nonlinear optimization and optimal control,” Mathematical Programming Computation, vol.11, pp.1–36, 2019. https://github.com/BorveErik/Autonomous-Truck-Sim (to be published on or after 17 June 2023) United Nations Agreement Concerning the Adoption of Uniform Technical Prescriptions for Wheeled Vehicles, Equipment and Parts which can be fitted and/or be used on Wheeled Vehicles and the Conditions for Reciprocal Recognition of Approvals Granted on the Basis of these Prescriptions, Addendum 47 (Regulation No.48) on Uniform provisions concerning the approval of vehicles with regard to the installation of lighting and light- signaling devices, E/ECE/324/Rev.1/Add.47/Rev.12 A. Kesting, M. Treiber, and D. Helbing, “General lane-changing model MOBIL for car-following models”, Transportation Research Record, vol.1999, no.1, pp.86–94, 2007

Claims

CLAIMS 1. A computer-implemented method (400) of controlling an ego vehicle (110) in a multi-lane driving environment (100), wherein the method comprises performing the following by means of processing circuitry in a computer system: sensing (412) position data of one or more surrounding vehicles (120), which are included in the multi-lane driving environment; planning (414) a trajectory of the ego vehicle in the multi-lane driving environment; predicting (416) trajectories of the surrounding vehicles for at least the duration of the trajectory of the ego vehicle, wherein the prediction is based on the sensed position data of the surrounding vehicles; updating (418), at least once, the trajectory of the ego vehicle in view of the predicted trajectories of the surrounding vehicles; and controlling (422) movements of the ego vehicle based on the updated trajectory, wherein the prediction of a trajectory of one of the surrounding vehicles comprises considering (416.2) the ego vehicle’s influence on a longitudinal acceleration of said surrounding vehicle.

2. The method (400) of claim 1, wherein the prediction (416) of a trajectory of one of the surrounding vehicles comprises simulating the ego vehicle’s influence based on a lateral closeness of the surrounding vehicle.

3. The method of claim 2, wherein the lateral closeness corresponds to a minimum lateral separation (^̂) of the ego vehicle and the surrounding vehicle in the lateral direction of the ego vehicle.

4. The method of claim 3, wherein: the ego vehicle is a combination of two or more vehicle units; and the lateral closeness is determined by minimizing, over all vehicle units, a minimum lateral separation (^̂) of the respective vehicle unit and the surrounding vehicle in the lateral direction of the vehicle unit.

5. The method of claim 3 or 4, wherein the minimum lateral separation (^̂) is determined from a point on the ego vehicle’s body which minimizes the lateral distance to the surrounding vehicle.

6. The method of any of claims 3 to 5, wherein the ego vehicle’s influence on the longitudinal acceleration of the surrounding vehicle is simulated based on a speed of the surrounding vehicle, a speed difference relative to the ego vehicle, and the minimum lateral separation (^̂).

7. The method of claim 6, wherein the simulated longitudinal acceleration of the surrounding vehicle is inversely proportional to the square of the minimum lateral separation (^̂).

8. The method of any of the preceding claims, wherein the prediction (416) of a trajectory of one of the surrounding vehicles comprises simulating the ego vehicle’s influence based on the ego vehicle’s use of a turning indicator, preferably based on the use of a turning indicator in the direction of the surrounding vehicle.

9. The method of any of the preceding claims, further comprising: receiving (410) a lane-change instruction, which is to be considered in the planning of the trajectory of the ego vehicle.

10. The method of any of the preceding claims, wherein updating (418) the trajectory of the ego vehicle comprises: re-planning (418.2) the trajectory of the ego vehicle in the multi-lane driving environment, in which the surrounding vehicles move according to the predicted trajectories; and interpolating (418.4) between the existing trajectory and the re-planned trajectory of the ego vehicle.

11. The method of claim 10, wherein the existing trajectory and the re-planned trajectory of the ego vehicle are interpolated (418.4) by a convex combination.

12. The method of any of the preceding claims, wherein the prediction of a trajectory of one of the surrounding vehicles further comprises considering the ego vehicle’s influence on a lane-change behavior of said surrounding vehicle.

13. The method of any of the preceding claims, which is performed in real time during operation of the ego vehicle.

14. The method of any of the preceding claims, which is performed by an entity not authorized to directly control movements of the surrounding vehicles.

15. The method of any of the preceding claims, wherein planning the trajectory of the ego vehicle comprises executing a model-predictive control, MPC, algorithm.

16. The method of any of the preceding claims, wherein predicting the trajectories of the surrounding vehicles comprises executing one or more of: a model-based simulation; a distributed MPC algorithm; a machine-learning predictor.

17. The method of any of the preceding claims, wherein the position data includes one or more of: position, pose, linear speed, angular speed, linear acceleration, angular acceleration.

18. The method of any of the preceding claims, wherein each trajectory of the ego vehicle includes a position sequence and a control-signal sequence.

19. A computer system (500) for controlling an ego vehicle (110) in a multi-lane driving environment (100), the computer system comprising processing circuitry (502) configured to: sense position data of one or more surrounding vehicles (120), which are included a multi-lane driving environment (100); plan a trajectory of the ego vehicle in the multi-lane driving environment; predict trajectories of the surrounding vehicles for at least the duration of the trajectory of the ego vehicle, wherein the prediction is based on the sensed position data of the surrounding vehicles; update, at least once, the trajectory of the ego vehicle in view of the predicted trajectories of the surrounding vehicles; and control movements of the ego vehicle based on the updated trajectory, wherein the prediction of the trajectory of one of the surrounding vehicles includes considering the ego vehicle’s influence on a longitudinal acceleration of said surrounding vehicle.

20. A computer program (520) comprising program code for performing, when executed by processing circuitry (502), the method of any of claims 1 to 18.

21. A non-transitory computer-readable storage medium comprising instructions, which when executed by processing circuitry (502), cause the processing circuitry to perform the method of any of claims 1 to 18.