Systems and methods for vehicle decision making and motion planning using real-time mixed integer programming

Abstract

A vehicle is controlled to travel on a road, the road having a geometric design defined by one or a combination of road alignment, profile, and cross section, and different traffic situations are defined by different values ​​of parameters of the road's geometric design, traffic on the road, and traffic rules for traffic flow on the road. The vehicle is controlled by transforming a mixed-integer non-convex constrained optimization problem for the current real-world traffic situation into a mixed-integer convex optimization problem for an approximate representation of the real-world traffic situation by relaxing configuration parameters of the real-world situation and restricting corresponding constraint parameters. Current control commands for controlling one or more actuators of the vehicle are generated by solving the transformed mixed-integer convex optimization problem for the approximate representation of the real-world traffic situation.

Description

[Technical Field]

[0001] The present disclosure relates generally to optimization-based control, and more particularly to a method and apparatus for mixed integer model predictive control in vehicle decision-making and motion planning using continuous and discrete computing elements. [Background technology]

[0002] Conventional autonomous vehicles are equipped with a control system that determines how the vehicle should move on roads while satisfying legal driving and traffic regulations to achieve its driving objectives. Conventional control systems determine vehicle motion by analyzing the environment based on data acquired by sensors and processed by recognition and mapping algorithms, calculating a desired vehicle path and speed, and controlling the vehicle to follow that path using available vehicle actuators. Due to the complexity of such conventional operation, some conventional control systems include separate components responsible for path planning and vehicle control. For example, U.S. Patent No. 9,915,948, incorporated herein by reference, discusses how vehicle control and path planning can be integrated to ensure that the vehicle achieves the desired objectives of a (semi-)automated driving system.

[0003] For example, path planning for an autonomous vehicle may involve a motion planning system (MPS) responsible for determining the vehicle's path and / or motion trajectory. The MPS may use different path planning methods, see, for example, U.S. Pat. No. 9,568,915, incorporated herein by reference. To determine a motion trajectory to reach a target goal, the MPS may use current and predicted environmental knowledge and a map of the environment obtained from vehicle sensors or received via a communication channel. To be able to adjust the motion trajectory according to the changing environment, the MPS needs to continuously update the motion trajectory in real time with limited computation due to the limited capabilities of the computing and communication platforms in the vehicle.

[0004] Therefore, because it needs to operate in real time to take into account the dynamic and rapidly changing environment, the motion trajectory can only be predicted over a short period of time; i.e., the planning interval of the MPS cannot cover the entire driving path of the vehicle, but only a partial segment from the current position to the next intermediate driving goal. In complex dynamic situations such as autonomous driving in an urban environment, there may be a series of intermediate goals, all of which may lead to successful completion of the journey. On the other hand, some intermediate goals achievable by the vehicle may not achieve the target driving goal due to potential violations of traffic rules and / or collisions with other vehicles. Therefore, some intermediate goals may actually be impractical to achieve for vehicle motion, and if provided to the MPS, autonomous control of the vehicle will fail.

[0005] Thus, an autonomous vehicle may execute a decision-making system (DMS) configured to select intermediate goals to be tracked by the vehicle's motion trajectory on its path to a target destination, see, for example, U.S. Patent US20210302974, which is incorporated herein by reference. However, it is typically difficult for the DMS to determine intermediate goals achievable by the vehicle without being too conservative with respect to vehicle dynamics, traffic rules, and avoiding collisions with any other traffic parties in a dynamically changing environment, thereby reducing the optimality of the automated driving system.

[0006] Therefore, what is needed is a simultaneous vehicle decision-making and motion planning system that makes discrete decisions regarding a series of intermediate goals while simultaneously computing sequential actions to optimally control a motion trajectory that achieves the overall goal of the automated driving system.

[0007] Optimization-based decision-making, planning, and control techniques, such as model predictive control (MPC), enable a model-based design framework that can directly consider system dynamics, system requirements, and constraints. This framework has been extended to hybrid dynamic systems containing both continuous and discrete decision variables, providing a powerful technique for modeling a wide variety of problems, including, for example, dynamic systems with switched modes or quantized actuation, problems with logic rules, temporal logic specifications, or obstacle avoidance constraints. However, the resulting optimization problems are highly nonconvex because they involve variables that take only integer values, and therefore are difficult to solve in practice. When linear or linear-quadratic objectives are used in combination with linear system dynamics and linear inequality constraints, the resulting optimal control problem (OCP) can be formulated as a mixed-integer linear program (MILP) or mixed-integer quadratic program (MIQP). More general convex inequality constraints can be included, such as quadratic inequality constraints resulting in a mixed-integer quadratically constrained quadratic program (MIQCQP), or quadratic cone constraints resulting in a mixed-integer second-order cone program (MISOCP).

[0008] Mixed-integer model predictive control (MI-MPC) for simultaneous decision-making and motion planning requires the solution of a mixed-integer programming problem (MIP) at each sampling instant within strict timing constraints. This is a challenging task given that solving MIPs is generally NP-hard, and several methods for solving such sequences of MIPs have been explored in the literature. These methods can be divided into heuristic techniques that attempt to efficiently find a suboptimal solution to the problem and optimization algorithms that attempt to optimally solve the MIP. Most mixed-integer optimization algorithms are based on variations of the branch-and-bound (B&B) technique to optimally solve MIPs. Variations of the branch-and-bound technique have been combined with various methods for solving relaxed convex subproblems, such as dual active set solvers, interior-point algorithms, dual projected gradient methods, nonnegative least-squares solvers, and alternating direction method of multipliers (ADMM). However, the combinatorial complexity of MIPs generally leads to an exponential increase in the computational time of the B&B method for solving MIPs as the number of discrete decision variables increases, limiting the applicability of MIP-based optimal control design in practice.

[0009] Examples of heuristic search techniques can be based on round-and-pump methods, using approximate optimization algorithms, approximate dynamic programming, or using data-driven machine learning techniques, such as supervised learning. Using supervised learning to replicate optimal and feasible MIP solutions from offline B&B methods and rapidly inferring these solutions online has dramatically improved the solution time for mixed-integer optimal control problems (MIOCPs). Alternatively, reinforcement learning techniques have been used to learn tree search policies to accelerate B&B methods, but these methods have limited applicability in practice to real-time embedded systems because they require at least one forward pass of a predictor, such as a neural network, at each node of the B&B tree. More importantly, they can still require the complete B&B tree to be enumerated in the worst case.

[0010] To that end, there is a need for a computationally efficient approach that ensures finding a feasible, but potentially suboptimal, solution to the MIP at each sampling instant, while meeting real-time requirements with limited computation due to the limited capabilities of the computational and communication platforms within the vehicle.The present invention describes a system and method for simultaneous decision-making and motion planning in (semi-)autonomous vehicles using a combination of a tailored branch-and-bound (B&B) method, including pre-solution reduction techniques and optimization algorithms for solving the convex relaxation of the MIP, and the use of heuristic search techniques to compute a feasible, but potentially suboptimal, solution, e.g., based on (supervised) machine learning, that accelerates the MIP solution. Summary of the Invention

[0011] An objective of some embodiments is to provide a simultaneous decision-making and motion planning system (DM-MPS) for an autonomous vehicle configured to select discrete decisions regarding a series of intermediate objectives while calculating continuous actions to optimally control a motion trajectory that achieves the overall goal of the automated driving system. For example, in some embodiments of the present invention, the proposed system aims to follow a route from the vehicle's current position to a desired destination in a transportation network that may include one or more road segments, each including one or more lanes, one or more intersections, and in a dynamically changing environment including one or more other traffic parties. Thus, an objective of some embodiments is for the DM-MPS to follow the route while satisfying the vehicle's kinematic constraints, satisfying traffic rules, and avoiding collisions with any other traffic parties in the environment.

[0012] Additionally or alternatively, an objective of some embodiments is to provide such a DM-MPS in which simultaneous calculation of discrete and continuous decisions is suitable for execution in real time by an autonomous vehicle computation unit, in order to take into account the need for frequent recalculation of discrete decisions and continuous motion trajectories according to a dynamically changing environment. Additionally, the DM-MPS must always ensure safety, i.e., simplification of calculations should not jeopardize the safety of the actual control of the vehicle. However, a DM-MPS that achieves each of the aforementioned objectives is difficult to design in practice.

[0013] Some embodiments of the present invention are based on transforming the vehicle and its dynamically changing environment from a real-world coordinate system to a road-aligned coordinate system to simplify the calculations at each sampling time instant of the DM-MPS. In some embodiments of the present invention, the transformation of the vehicle's predictive model results in updated boundary constraints, e.g., updated limits on the vehicle's steering and / or lateral speed to take into account the curvature of road segments along the vehicle's route. In addition, by transforming each of the traffic parties and their predictive models and traffic rules into a road-aligned coordinate system, safety and traffic rules can be met by the DM-MPS.

[0014] Some embodiments of the present invention are based on the recognition that transformation to a road-aligned coordinate system enables simultaneous computation of discrete decisions and continuous actions in a DM-MPS system, which can be formulated as a structured mixed-integer linear programming (MILP) problem or a structured mixed-integer quadratic programming (MIQP) problem that can be solved efficiently in real time. Additionally, in some embodiments of the present invention, after solving the MIP by the DM-MPS system at each sampling instant, the computed motion trajectory is transformed back from the road-aligned coordinate system to a real-world coordinate system so that the transformed motion trajectory can be executed by a vehicle control system. In some embodiments of the present invention, the vehicle control system is implemented using a model predictive controller (MPC) that aims to follow the continuous reference trajectory computed by the DM-MPS.

[0015] In some embodiments of the present invention, predictions of future positions and / or future velocities for one or more traffic parties in the environment must be performed by the DM-MPS to calculate optimal and safe discrete decisions and continuous actions for the autonomous vehicle over a prediction time window. Some embodiments of the present invention are based on the recognition that an increasing safety margin must be met around future predictions for the traffic parties to ensure robust safety given a changing environment, perception errors, unknown disturbances, modeling, and prediction errors. Furthermore, the DM-MPS must continuously update the discrete decisions and motion trajectories for the autonomous vehicle in real time to be robust to these changes, disturbances, and errors in the environment.

[0016] In some embodiments of the present invention, a DM-MPS system is implemented by solving a block-structured MILP or MIQP at each sampling instant using a combination of a tailored branch-and-bound (B&B) method, including pre-solution reduction techniques and optimization algorithms for solving the convex relaxation of the MIP, and the use of heuristic search techniques to compute feasible, but potentially suboptimal, solutions, e.g., based on (supervised) machine learning, to accelerate the MIP solution. Examples of optimization algorithms for solving convex relaxations include the active set method, interior point method, (projected) gradient method, and alternating direction method of multipliers (ADMM).

[0017] Some embodiments of the present invention are based on the recognition that pre-solution reduction techniques can be used to reduce the number of decision variables and / or the number of constraints in a MIP while maintaining feasibility and optimality. In some embodiments of the present invention, a pre-solution procedure is used at each node of the B&B method to reformulate the MIP into a reduced MIP with fewer variables and / or constraints, where the reduced MIP is infeasible or unbounded only if the original MIP is infeasible or unbounded, and any feasible or optimal solution of the reduced MIP can be mapped to a feasible or optimal solution of the original MIP. Examples of pre-solution reduction techniques include domain propagation, bound tightening, double fixing, implicit variable substitution, coefficient tightening, probing, and detection and removal of redundant variables and / or redundant constraints.

[0018] Some embodiments of the present invention are based on the recognition that a mixed integer convex programming (MICP) problem can be efficiently solved as a convex program (CP) after fixing all discrete variables to a fixed set of values ​​provided by a predictor, i.e., after fixing all binary variables to 0 or 1 and all integer variables to integer values. Examples of predictors can be based on (supervised) machine learning, heuristic pre-solving techniques, or warm starts based on MIP solutions at previous sampling times. Some embodiments of the present invention calculate the values ​​of the discrete variables θ at each of the time steps i=0, 1,...,N in the control horizon of the MIP, given a set of values ​​for the problem parameter θ, to compute a feasible, but possibly suboptimal, solution to the MIP at the current sampling time.

number

[0019] Some embodiments are based on the recognition that due to the complexity of real-world traffic situations, optimizing simultaneous decision-making and motion planning for vehicles traveling on roads requires the solution of a mixed-integer non-convex constrained optimization problem, which is difficult to solve in real time. Unfortunately, the geometric design of the road, defined by one or a combination of the road's alignment, profile, and cross-section, the traffic on the road formed by vehicles and other vehicles and pedestrians on the road, and the traffic rules for the flow of traffic on the road that restrict the operation of vehicles, form a very complex and variable traffic situation.

[0020] In some embodiments of the invention, examples of mixed-integer non-convex constrained optimization problems include nonlinear vehicle dynamics and / or nonlinear road boundary constraints in combination with mixed-integer equality and / or inequality constraints, enforcing traffic rules such as, for example, collision avoidance constraints, lane change constraints, and / or traffic intersection stopping constraints. Examples of real-world traffic situations for vehicle decision-making and motion planning may include (semi-)autonomous vehicles and one or more other vehicles traveling in a complex environment consisting of one or more connected road segments, with one or more lanes, one or more speed zones, one or more traffic intersections, one or more stopping zones, one or more traffic lights, and / or one or more merging points.

[0021] Some embodiments are based on the recognition that each real-world traffic situation can be represented by a set of parameters. Some of the parameters, such as road curvature or vehicle shape, may cause non-convexity in the optimization problem. Other parameters, such as constraints on longitudinal speed and / or acceleration and constraints on lateral speed and / or acceleration, are independent of the non-convexity, i.e., do not cause non-convexity in the optimization problem. For example, in some embodiments of the present invention, a convex approximation of a non-linear vehicle kinematics model is used that includes a constraint on lateral speed that depends on longitudinal speed to avoid non-convexity in the optimization problem.

[0022] Some embodiments are based on the recognition that parameters that cause non-convexity can be relaxed at the expense of parameters that are independent of the non-convexity. For example, a parameter for road curvature can be relaxed to straighten the road by restricting limits on the vehicle's lateral speed and / or lateral acceleration. In another example, a parameter for left or right turns at a traffic intersection can be relaxed to straighten the road by restricting limits on the vehicle's lateral position and / or by restricting limits on the vehicle's lateral speed and / or lateral acceleration. Similarly, a parameter for vehicle steering speed limits can be relaxed to linearize the vehicle model by restricting limits on the vehicle's lateral speed and / or lateral acceleration. As yet another example, a parameter for the vehicle's physical shape can be relaxed by restricting limits on the vehicle's position due to collision avoidance constraints. In this way, the mixed integer non-convex constrained optimization problem can be transformed into a mixed integer convex constrained optimization problem by relaxing parameters of the current real-world traffic situation that cause the non-convexity of the mixed integer non-convex constrained optimization problem, and by regulating at least some parameters of the current real-world traffic situation that are independent of the non-convexity of the mixed integer non-convex constrained optimization problem.

[0023] In some embodiments of the present invention, the proposed system and method for vehicle decision-making and motion planning uses, at each control time step, a transformation from one or more parameters in a real-world traffic situation to relaxing configuration parameters and restricting one or more constraint parameters in a mixed-integer convex programming (MICP) approximate representation of the real-world traffic situation. In some embodiments of the present invention, the transformation of a real-world traffic situation of a vehicle motion planning problem involving one or more other vehicles on a curved road segment leads to an approximate representation of the vehicle motion planning problem involving one or more other vehicles on a straight road segment with restricted constraint parameter values, for example, restricted boundary values ​​of one or more constraint functions in a MICP-based approximate representation of the traffic situation involving a (piecewise) linear dynamic model of the (semi-)autonomous vehicle and a simplified representation of the vehicle's environment in the traffic situation. The MICP solution includes an optimal motion trajectory for the vehicle, which is further transformed from the approximate representation of the traffic situation back to the real-world representation in accordance with some embodiments of the present invention to control the motion of the (semi-)autonomous vehicle in the real-world traffic situation.

[0024] Some embodiments of the present invention are based on the recognition that the MICP problem is a convex constrained optimization problem that is computationally inexpensive to solve for a fixed set of values ​​for each of the integer variables, and therefore computationally efficient branch-and-bound and / or machine learning based techniques can be used to efficiently compute the fixed set of values ​​for each of the integer variables in the optimal MICP solution.

[0025] Accordingly, one embodiment provides a controller for controlling a vehicle traveling on a road, the road having a geometric design defined by one or a combination of an alignment, a profile, and a cross section of the road, different traffic situations being defined by different values ​​of parameters of the geometric design of the road, traffic on the road, and traffic rules for the flow of traffic on the road, the controller comprising at least one processor and a memory having stored thereon instructions that, when executed by the at least one processor, cause the controller to calculate configuration parameters that induce non-convexity in a mixed integer non-convex constrained optimization problem for simultaneous decision making and motion planning of the vehicle, and calculate parameters that induce non-convexity in a mixed integer non-convex constrained optimization problem for simultaneous decision making and motion planning of the vehicle. a constraint parameter that is independent of the non-convexity of the constrained optimization problem; converting the mixed-integer non-convex constrained optimization problem for the current real-world traffic condition into a mixed-integer convex optimization problem for an approximate representation of the real-world traffic condition by relaxing the configuration parameters and restricting corresponding constraint parameters; solving the converted mixed-integer convex optimization problem for the approximate representation of the real-world traffic condition to generate current control commands for controlling one or more actuators of the vehicle; and controlling the one or more actuators of the vehicle according to the control commands.

[0026] Another embodiment is a method for controlling a vehicle traveling on a road, the road having a geometric design defined by one or a combination of an alignment, a profile, and a cross section of the road, different traffic situations defined by different values ​​of parameters of the geometric design of the road, traffic on the road, and traffic rules for the flow of traffic on the road, the method using a processor coupled to a memory having instructions stored thereon that, when executed by the processor, perform steps of the method, the steps including: determining configuration parameters that cause non-convexity in a mixed integer non-convex constrained optimization problem for simultaneous decision making and motion planning of the vehicle; and determining the non-convexity in the mixed integer non-convex constrained optimization problem. a mixed integer non-convex constrained optimization problem for the current real-world traffic condition to generate a current control command for controlling one or more actuators of the vehicle; and a constraint parameter that is convexity independent. The method includes collecting parameters of a current real-world traffic condition for controlling the vehicle, the parameter including a constraint parameter that is convexity independent; converting the mixed integer non-convex constrained optimization problem for the current real-world traffic condition into a mixed integer convex optimization problem for an approximate representation of the real-world traffic condition by relaxing the configuration parameters and restricting corresponding constraint parameters; solving the converted mixed integer convex optimization problem for the approximate representation of the real-world traffic condition to generate a current control command for controlling one or more actuators of the vehicle; and controlling the one or more actuators of the vehicle according to the control command.

[0027] Yet another embodiment is a non-transitory computer-readable storage medium having embodied thereon a program executable by a processor for performing a method for controlling a vehicle traveling on a road, the road having a geometric design defined by one or a combination of an alignment, a profile, and a cross-section of the road, different traffic situations being defined by different values ​​of parameters of the geometric design of the road, traffic on the road, and traffic rules for the flow of traffic on the road, the method comprising: configuring parameters that cause non-convexity of a mixed-integer non-convex constrained optimization problem for simultaneous decision-making and motion planning of the vehicle; and limiting parameters that are independent of the non-convexity of the mixed-integer non-convex constrained optimization problem. collecting parameters of a current real-world traffic situation for controlling the vehicle, the method comprising: converting the mixed integer non-convex constrained optimization problem for the current real-world traffic situation into a mixed integer convex optimization problem for an approximate representation of the real-world traffic situation by relaxing the configuration parameters and restricting corresponding constraint parameters; solving the converted mixed integer convex optimization problem for the approximate representation of the real-world traffic situation to generate current control commands for controlling one or more actuators of the vehicle; and controlling the one or more actuators of the vehicle according to the control commands. [Brief explanation of the drawings]

[0028] [Figure 1A] FIG. 1 illustrates a block diagram of a mixed integer predictive controller and feedback system according to some embodiments. [Figure 1B] FIG. 1 illustrates a block diagram of an implementation of a mixed integer predictive controller and feedback system according to some embodiments of the present invention. [Figure 1C] FIG. 1 illustrates a block diagram of an implementation of a mixed integer predictive controller and feedback system according to some embodiments of the present invention. [Figure 1D]FIG. 1 illustrates a block diagram of a multi-layer control architecture and feedback system, including a vehicle decision-making and motion planning system, according to some embodiments of the present invention. [Figure 2A] 1 shows a schematic diagram of a vehicle including a predictive controller employing principles of some embodiments. [Figure 2B] 1 illustrates a schematic diagram of interactions between a predictive controller and other controllers of a vehicle, according to some embodiments. [Figure 2C] 1 shows a schematic diagram of a path and / or motion planning method for a controlled vehicle employing principles of some embodiments. [Figure 2D] 1 illustrates an example traffic scene for a single-vehicle or multi-vehicle decision-making module according to some embodiments. [Figure 3A] FIG. 1 illustrates a block diagram of a transformation from a real-world representation to an approximate representation of a constrained optimization problem in a vehicle decision-making and motion planning system for a controlled vehicle, according to some embodiments of the present invention. [Figure 3B] Figure 1 shows a block diagram of the transformation and solving strategy, including a first transformation step, solving the MICP problem, followed by a second inverse transformation step to calculate the optimal motion trajectory and sequence of control actions in a real-world traffic situation. [Figure 3C] FIG. 1 illustrates a block diagram of a transformation and solution strategy that includes relaxing one or more configuration parameters that cause non-convexity, and includes regulating one or more limiting parameters that are independent of non-convexity in a constrained optimization problem for vehicle decision-making and motion planning, according to some embodiments. [Figure 4A] We show the transformation of a vehicle decision-making and motion planning problem in an exemplary traffic situation with one or more other vehicles on a curved road segment from a real-world representation to an approximate representation with regulated, restricted parameter values ​​in a constrained mixed-integer convex optimization problem. [Figure 4B] 1 illustrates the inverse transformation of an optimal solution to a mixed integer convex optimization problem with constrained parameter values ​​into an approximate solution to a mixed integer non-convex optimization problem for a real-world representation, according to some embodiments. [Figure 5] 1 illustrates transformation and inverse transformation steps for a vehicle decision-making and motion planning problem in an exemplary traffic situation of a controlled vehicle with one or more other vehicles in the vicinity of a traffic intersection, according to some embodiments of the present invention. [Figure 6A] FIG. 1 illustrates a block diagram of a system and method for a mixed-integer model predictive controller (MI-MPC) to compute control signals given the current state of the system and control commands in a proposed vehicle decision-making and motion planning system, according to some embodiments. [Figure 6B] A block diagram of an MI-MPC method for solving an optimal control structured mixed integer linear-quadratic optimization problem is shown, which calculates control signals at each control time step given the current state and commands of the vehicle, and computes a motion plan that includes a series of future discrete decisions and continuous actions for the controlled vehicle. [Figure 6C] 1 shows a block diagram of an MI-MPC method for finding a feasible, but potentially suboptimal, solution vector for an optimal control structured MICP to compute control signals at each time step given current problem parameter values ​​and commands to compute a motion plan comprising a series of future discrete decisions and continuous actions for a controlled vehicle, according to some embodiments of the present invention. [Figure 7A] 1 illustrates one or more obstacle avoidance constraints using mixed-integer inequality constraints that force a controlled vehicle to stay inside one of multiple disjoint regions outside a safety region around an obstacle in a transportation network, according to some embodiments of the present invention. [Figure 7B] 1 illustrates one or more obstacle avoidance constraints used in the proposed vehicle decision-making and motion planning system for a controlled vehicle in an approximate representation of an exemplary real-world traffic situation involving one or more other vehicles. [Figure 7C]1 illustrates one or more obstacle avoidance constraints used in the proposed vehicle decision-making and motion planning system in an approximate representation of an exemplary real-world traffic situation involving one or more other vehicles, as well as one or more traffic intersections along a particular route of a controlled vehicle in a transportation network. [Figure 7D] 1 illustrates one or more spatially dependent zone constraints used in the proposed vehicle decision-making and motion planning system for a controlled vehicle in an approximate representation of an exemplary real-world traffic situation involving one or more other vehicles, according to some embodiments of the present invention. [Figure 8A] FIG. 1 shows a schematic diagram of an example binary control-variable search tree representing a tree of nested search spaces for integer feasible control solutions, according to some embodiments of the present invention. [Figure 8B] FIG. 1 illustrates a block diagram of a branch-and-bound mixed-integer optimization algorithm for searching for integer-feasible optimal control solutions based on a tree of nested search spaces and corresponding lower / upper bound values, in accordance with some embodiments of the present invention. [Figure 9A] We show that, according to some embodiments of the present invention, after fixing all discrete variables to a fixed set of values, an optimal control structured MICP problem can be transformed and solved very efficiently as an optimal control structured convex programming (CP) problem. [Figure 9B] FIG. 1 shows a block diagram of MICP solving based on prediction followed by modification of a set of fixed discrete values ​​and solving an optimal control structured CP according to some embodiments. [Figure 9C] FIG. 1 shows a block diagram of MICP solving based on multiple predictions, each followed by the modification of a set of fixed discrete values ​​and the solution of an optimal control structured CP, according to some embodiments. [Figure 9D] FIG. 1 shows a block diagram of a MICP solution based on predictions used to fix a first subset of discrete variables, followed by pre-solution-based modifications to fix the remaining subset of discrete variables, and the solution of an optimal control structured CP, according to some embodiments. [Figure 10A]FIG. 1 shows a block diagram of an offline data generation and supervised learning procedure for training a machine learning-based predictor used in an online variable fixing and optimal control solving procedure for solving MICP in a vehicle decision-making and motion planning system. [Figure 10B] FIG. 1 shows a flow diagram of an offline data generation procedure based on a MICP solution for a set of sampled problem parameter values ​​and a supervised learning procedure for training a machine learning-based predictor according to some embodiments of the present invention. [Figure 10C] 1 illustrates a flow diagram of an online variable fixing procedure based on prediction and iterative pre-solution-based correction steps and an optimal control structured CP solving procedure for computing MICP solutions in a vehicle decision-making and motion planning system, according to some embodiments. DETAILED DESCRIPTION OF THE INVENTION

[0029] Some embodiments of the present disclosure provide systems and methods for controlling vehicle operation using a predictive controller. An example of a predictive controller is a model predictive controller (MPC), which determines control inputs based on a model of the controlled vehicle. Another example of a predictive controller is a mixed-integer model predictive controller (MI-MPC) for vehicle decision-making, motion planning, and / or trajectory generation, which involves continuous and discrete computing elements.

[0030] Some embodiments are based on the recognition that due to the complexity of real-world traffic situations, optimizing simultaneous decision-making and motion planning for vehicles traveling on roads requires the solution of a mixed-integer non-convex constrained optimization problem, which is difficult to solve in real time. Unfortunately, the geometric design of the road, defined by one or a combination of the road's alignment, profile, and cross-section, the traffic on the road formed by vehicles and other vehicles and pedestrians on the road, and the traffic rules for the flow of traffic on the road that restrict the operation of vehicles, form a very complex and variable traffic situation.

[0031] In some embodiments of the invention, examples of mixed-integer non-convex constrained optimization problems include nonlinear vehicle dynamics and / or nonlinear road boundary constraints in combination with mixed-integer equality and / or inequality constraints, enforcing traffic rules such as, for example, collision avoidance constraints, lane change constraints, and / or traffic intersection stopping constraints. Examples of real-world traffic situations for vehicle decision-making and motion planning may include (semi-)autonomous vehicles and one or more other vehicles traveling in a complex environment consisting of one or more connected road segments, with one or more lanes, one or more speed zones, one or more traffic intersections, one or more stopping zones, one or more traffic lights, and / or one or more merging points.

[0032] Some embodiments are based on the recognition that each real-world traffic situation can be represented by a set of parameters. Some of the parameters, such as road curvature or vehicle shape, may cause non-convexity in the optimization problem. Other parameters, such as constraints on longitudinal speed and / or acceleration and constraints on lateral speed and / or acceleration, are independent of the non-convexity, i.e., do not cause non-convexity in the optimization problem. For example, in some embodiments of the present invention, a convex approximation of a non-linear vehicle kinematics model is used that includes a constraint on lateral speed that depends on longitudinal speed to avoid non-convexity in the optimization problem.

[0033] Some embodiments are based on the recognition that parameters that cause non-convexity can be relaxed at the expense of parameters that are independent of the non-convexity. For example, a parameter for road curvature can be relaxed to straighten the road by restricting limits on the vehicle's lateral speed and / or lateral acceleration. In another example, a parameter for left or right turns at a traffic intersection can be relaxed to straighten the road by restricting limits on the vehicle's lateral position and / or by restricting limits on the vehicle's lateral speed and / or lateral acceleration. Similarly, a parameter for vehicle steering speed limits can be relaxed to linearize the vehicle model by restricting limits on the vehicle's lateral speed and / or lateral acceleration. As yet another example, a parameter for the vehicle's physical shape can be relaxed by restricting limits on the vehicle's position due to collision avoidance constraints. In this way, the mixed integer non-convex constrained optimization problem can be transformed into a mixed integer convex constrained optimization problem by relaxing parameters of the current real-world traffic situation that cause the non-convexity of the mixed integer non-convex constrained optimization problem, and by regulating at least some parameters of the current real-world traffic situation that are independent of the non-convexity of the mixed integer non-convex constrained optimization problem.

[0034] In some embodiments of the present invention, the proposed system and method for vehicle decision-making and motion planning uses, at each control time step, a transformation from one or more parameters in a real-world traffic situation to relaxing configuration parameters and restricting one or more constraint parameters in a mixed-integer convex programming (MICP) approximate representation of the real-world traffic situation. In some embodiments of the present invention, the transformation of a real-world traffic situation of a vehicle motion planning problem involving one or more other vehicles on a curved road segment leads to an approximate representation of the vehicle motion planning problem involving one or more other vehicles on a straight road segment with restricted constraint parameter values, for example, restricted boundary values ​​of one or more constraint functions in a MICP-based approximate representation of the traffic situation involving a (piecewise) linear dynamic model of the (semi-)autonomous vehicle and a simplified representation of the vehicle's environment in the traffic situation. The MICP solution includes an optimal motion trajectory for the vehicle, which is further transformed from the approximate representation of the traffic situation back to the real-world representation in accordance with some embodiments of the present invention to control the motion of the (semi-)autonomous vehicle in the real-world traffic situation.

[0035] Some embodiments of the present invention are based on the recognition that the MICP problem is a convex constrained optimization problem that is computationally inexpensive to solve for a fixed set of values ​​for each of the integer variables, and therefore computationally efficient branch-and-bound and / or machine learning based techniques can be used to efficiently compute the fixed set of values ​​for each of the integer variables in the optimal MICP solution.

[0036] 1A illustrates a block diagram of a predictive controller 110 and a feedback system for a controlled (semi-)autonomous vehicle 120, according to some embodiments. FIG. 1A illustrates an exemplary feedback system including a vehicle 120 connected to the predictive controller 110 via a state estimator 130 and one or more sensors and communication devices 140 for obtaining feedback signals from a traffic environment 145 of the vehicle 120, according to some embodiments of the present invention. Examples of feedback signals from the traffic environment 145 include the state of one or more neighboring vehicles around the controlled vehicle 120, including their position, shape, and / or speed. Other examples of feedback signals from the environment 145 include one or more traffic light signals, which define whether the controlled vehicle 120 is allowed to continue driving or is required to stop at a desired crossing direction through a traffic intersection. Other examples of feedback signals from the environment 145 include one or more traffic signs, including, for example, a stop sign, a speed limit sign, and / or a priority traffic sign. In some embodiments of the present invention, the sensor and communication device 140 provides information regarding the geometric design of the road, defined by one or a combination of the road's alignment, profile, and cross section, the traffic on the road formed by the vehicle and other vehicles and pedestrians on the road, and traffic rules that restrict the operation of the vehicle 120 with respect to the flow of traffic on the road.

[0037] In some implementations of the present invention, the predictive controller 110 is an MPC controller programmed according to a dynamic model 102 (or system model) of the controlled vehicle 120. The system model 102 may be a set of equations that describe the change in the state and output 103 of the vehicle 120 over time as a function of current and previous inputs 111 and previous outputs 103. The system model 102 may include constraints 104 that represent the physical and operational limitations of the system 120. During operation, the controller 110 receives commands 101 that indicate the desired behavior of the vehicle 120. The commands may be, for example, movement commands and / or routes within a transportation network to travel from a current location to a desired destination. In response to receiving the commands 101, the controller 110 generates control signals 111 that serve as inputs to the controlled vehicle 120. In response to this input, the system updates the output 103 of the vehicle 120. Based on measurements of the output 103 of the vehicle 120, an estimator 130 updates the estimated state 121 of the vehicle 120. This estimated state 121 of the vehicle 120 provides state feedback to the predictive controller 110. Thus, the predictive controller 110 receives a feedback signal 121 of the vehicle 120 via an estimator 130, the feedback signal 121 including measurements of the state of the vehicle 120, and the predictive controller receives one or more feedback signals 122 about the traffic environment 145 of the vehicle 120 using a sensor and communication device 140.

[0038] The controlled system 120 referred to herein may be any vehicle, including a two-wheeled vehicle such as a motorbike, a four-wheeled vehicle such as a car, or a vehicle with more than four wheels such as a truck. The vehicle 120 is controlled by certain operational input signals, e.g., control signals 111 (inputs), possibly related to physical quantities such as voltage, pressure, force, torque, etc., and returns several controlled output signals 103 (outputs), possibly related to physical quantities such as wheel speed, angular velocity, acceleration, velocity, position, etc., that indicate a transition of the state of the controlled vehicle 120 from a previous state to a current state. The output values ​​are related in part to previous output values ​​of the system and in part to previous and current input values. The dependency on previous inputs and previous outputs is encoded in the state of the controlled vehicle 120.

[0039] The system model 102 may include a set of mathematical equations that describe how the system outputs change over time as a function of current and previous inputs and outputs. The state of the vehicle 120 is generally any time-varying set of information, e.g., a proper subset of current inputs and outputs and previous inputs and outputs, that, together with the model and future inputs of the system, can uniquely define the future motion of the system.

[0040] Controlled vehicle 120 may be subject to physical and specification constraints 104 that limit the range over which the outputs, inputs, and possibly states of system 120 are allowed to operate. Examples of constraints 104 include safety distance constraints, speed limits, acceleration and deceleration constraints, steering speed limits, turning radius constraints, lane change constraints, and / or lane change timing constraints.

[0041] The controller 110, which may be implemented in hardware or as a software program running on a processor, e.g., a microprocessor, receives an estimated state 121 of the vehicle 120, one or more feedback signals 122 of the traffic environment 145, and a desired motion command 101 at fixed or variable control period sampling intervals, and uses this information to determine inputs, e.g., control signals 111, for operating the vehicle 120. According to some embodiments, the controller 110 further solves a mixed-integer optimal control optimization problem using branch-and-bound (B&B) optimization, which searches for a globally optimal solution within a search space, to generate the control signals 111. B&B optimization iteratively divides the search space into a tree of nested regions to find (an approximation of) a globally optimal solution to a mixed-integer programming (MIP) problem. Furthermore, the tree of nested regions is formed by different convex relaxations of the integer variables of the mixed-integer optimal control optimization problem. The predictive controller 110 further controls the vehicle 120 to change the state of the vehicle 120 based on the control signals 111.

[0042] In some embodiments of the present invention, a feasible, but possibly suboptimal, approximation of the MIP solution is found using heuristic search techniques, e.g., using a rounding and pumping scheme, using an approximate optimization algorithm, using approximate dynamic programming, or using data-based machine learning techniques to calculate the control signal 111. For example, supervised learning can be used to train a deep neural network architecture to achieve a predictive model that maps a set of parameter values ​​to the MIP solution and / or the control signal 111. In some embodiments of the present invention, the predictive model may be deterministic, including, e.g., multilayer perceptrons, deep sets, convolutional neural networks (CNNs), recurrent neural networks (RNNs), kernel regression, support vector machines, and other machine learning algorithms, or any combination of such deterministic predictive models. Alternatively, in some embodiments of the present invention, the predictive model may be probabilistic, including, e.g., Bayesian neural networks, neural processes, Gaussian processes, Kriging interpolation, and other machine learning algorithms, or any combination of such deterministic and / or probabilistic predictive models.

[0043] The estimator 130 can be implemented in hardware or as a software program running on a processor, either the same or a different processor as the controller 110, that receives the output 103 of the system at fixed or variable control period sampling intervals and uses new and previous output measurements to determine an estimated state 121 of the vehicle 120.

[0044] 1B shows a block diagram of a predictive controller 110 and a feedback system 120 according to some embodiments. The predictive controller 110 operates the vehicle 120 such that the estimated state 121 and output 103 of the vehicle 120 comply with the commands 101. The controller 110 includes a computer, for example, in the form of a single central processing unit (CPU) or multiple CPU processors 151 connected to a memory 152 for storing a system model 102 and constraints 104 for the operation of the controlled vehicle 120. The CPU processor 151 may consist of a single-core processor, a multi-core processor, a computing cluster, or any number of other configurations. The memory 152 may include random access memory (RAM), read-only memory (ROM), flash memory, or any other suitable memory system.

[0045] 1C shows a block diagram of the predictive controller 110 and feedback system 120, in which the memory 152 includes one or more sections, according to some embodiments. In some embodiments of the invention, the memory 152 includes a first section 152a for storing information about the vehicle, a second section 152b for storing programs for controlling the vehicle, a third section 152c for storing driving map data, and a fourth section 152d for storing a traffic motion model.

[0046] For example, the first section 152a of the memory 152 may store parameters for vehicle behavior, such as maximum acceleration, steering, and steering speed, as well as a first model of vehicle motion and a second model of vehicle motion. In various embodiments, the number and complexity of equations describing the second model of vehicle motion are greater than the number and complexity of equations describing the first model of vehicle motion. Also, for example, the fourth section 152d of the memory 152 may store a first model of traffic motion and a second model of traffic motion.

[0047] 1C , in various embodiments, the number and complexity of equations describing the second model of traffic motion are greater than the number and complexity of equations describing the first model of traffic motion. These embodiments are based on the recognition of the need to use different motion models to check which intermediate objectives a vehicle may be able to achieve in the near future and to generate trajectories and control the vehicle according to such trajectories. For example, to check whether a vehicle can achieve a set of objectives, a long future horizon needs to be considered. Having a high-order physics model to calculate the vehicle's motion over an extended future horizon is computationally intensive. Conversely, if the immediate objectives are known, controlling the vehicle according to the desired trajectory may only consider a short future horizon. To that end, in some embodiments, controller 110 determines the next objective using a first, or lower-order motion model, and planning and control uses at least a second, or higher-order motion model.

[0048] The second section 152b of the memory 152 may have embodied thereon a program executable by the processor 151 to perform a method for controlling the vehicle 120, according to some embodiments.

[0049] 1C, the third section 152c of the memory 152 contains map information such as addresses and road networks, which may also contain additional information such as intersections, stop and traffic light locations, number and location of lanes, speed limits, traffic rules, curvature of road segments, etc. The map information may already be stored in the third section of the memory 152c when the vehicle begins driving, or alternatively, this information is made available to the control unit by the sensor and communication device 140.

[0050] Processor 151 may be any computing device capable of performing calculations and may include one or more physical devices of the same or different types. Processor 151 may include multiple computing devices, such as microprocessors. Similarly, memory 152 may be any logical memory and / or non-transitory computer-readable storage medium capable of storing information and may include one or more physical information storage means of the same or different types. The calculations performed by processor 151 are directed by a program stored in the second section of memory 152b and use vehicle information stored in the first section of memory 152a, map information stored in the third section of memory 152c, information about vehicle 152a obtained from sensor output 103, and traffic information 152d of environment 145 obtained from sensor and communication device 140. The calculations of processor 151 result in control inputs 111 that alter the vehicle's motion.

[0051] FIG. 1D illustrates a block diagram of a feedback control system including a multi-layer architecture including one or more components coupled together to calculate a control signal 111 for controlling the motion of the vehicle 120, according to some embodiments of the present invention. The program executed by the processor 151 enables autonomous driving (AD) of the vehicle 120, where AD is intended to include semi-autonomous driving as well. During this operation, the program executed by the processor 151 aims to achieve a specific overall driving objective, such as reaching a specific location. The overall objective is achieved by appropriately influencing the motion of the vehicle 120. The software program executed by the processor 151 can be logically separated into multiple modules. For example, in one embodiment, the program executed by the processor includes at least two modules arranged in series as layers, such that the output of one layer is the input to the next layer. As used herein, such layering identifies layers or logical modules of the control unit 160, allowing for the separation of control into different stages requiring different information.

[0052] FIG. 1D shows a schematic diagram of the layers of the control unit 160 according to one embodiment of the present disclosure. In this embodiment, the control unit 160 includes four layers of control. Information about the state of the vehicle 121 and feedback signals 122 of the traffic environment 145 from the sensors and communications 140 are provided to each of the different layers. The routing module 161 uses map information stored in the third section of the memory 152c of FIG. 1C and the vehicle's current position obtained from the sensor output 103 to determine the sequence of roads in the road network that the vehicle should take to reach its desired destination from its current location. The desired destination may be provided, for example, by a human user. The routing module may be implemented by a car navigation system. The decision-making and motion planning module 162 aims to make discrete decisions regarding a series of intermediate goals while simultaneously calculating successive actions to optimally control a motion trajectory that achieves the overall goal of the automated driving system, according to some embodiments of the present invention.

[0053] In some embodiments of the present invention, the decision-making and motion planning module 162 uses information from the current state of the vehicle 121, information from the feedback signals 122 of the traffic environment 145, and information from at least a portion of the sequence of roads in the road network that the vehicle will traverse to reach its desired destination from its current location, and the module 162 determines a series of one or more intermediate goals and a series of one or more sequential actions, resulting in a feasible and / or optimal motion trajectory, which is provided to the vehicle control module 163. The vehicle control module 163 determines commands for vehicle actuators, such as steering, acceleration, deceleration, etc., that modify the vehicle behavior so that the vehicle achieves an actual trajectory as close as possible to the motion trajectory provided by the decision-making and motion planning module 162. The commands to the vehicle actuators are then received by the actuator control sub-module 164, which modifies control signals to the actuators, such as electric motor voltage, throttle opening, brake pad pressure, etc., to achieve the desired vehicle commands.

[0054] In some embodiments of the present invention, the decision-making and motion planning module 162 is implemented using an MI-MPC controller that solves a MIP at each sampling period and computes a series of discrete decisions and continuous control actions over a prediction time window. In some embodiments of the present invention, the vehicle control module 163 is implemented using a linear, linear time-varying, or nonlinear MPC controller that solves a convex quadratic programming (QP) problem or a nonconvex nonlinear programming (NLP) problem at each sampling period and computes a series of vehicle actuation commands to cause the vehicle to achieve an actual trajectory as close as possible to the maneuver trajectory. Some embodiments of the present invention are based on the recognition that the NLP can be solved using a sequential convex programming (SCP) technique or a sequential quadratic programming (SQP) technique, and that each convex QP subproblem can be solved using a convex optimization algorithm, such as an active set solver, an interior point algorithm, a projected gradient method, a nonnegative least squares solver, or an alternating direction multiplier method (ADMM).

[0055] Some embodiments of the present invention are based on the recognition that the decision making and motion planning module 162 uses a first model of vehicle motion in combination with a first model of traffic environment motion, and the vehicle control module 163 uses a second model of vehicle motion in combination with a second model of traffic environment motion. The number and complexity of equations describing the second model of vehicle motion is greater than the number and complexity of equations describing the first model of vehicle motion. The number and complexity of equations describing the second model of traffic motion is greater than the number and complexity of equations describing the first model of traffic motion.

[0056] In some embodiments of the present invention, the length of the prediction time window in decision-making and motion planning module 162 is longer than the length of the prediction time window in vehicle control module 163. Similarly, in some embodiments of the present invention, the length of the sampling period in decision-making and motion planning module 162 is longer than the length of the sampling period in vehicle control module 163. For example, in some embodiments of the present invention, the prediction time window is 10-20 seconds long for decision-making and motion planning module 162 and 2-4 seconds long for vehicle control module 163. Additionally, in some embodiments of the present invention, the sampling period is 0.25-0.5 seconds long for decision-making and motion planning module 162 and 0.025-0.05 seconds long for vehicle control module 163.

[0057] 2A shows a schematic diagram of a vehicle 201 including a predictive controller 202 employing principles of some embodiments of the present invention. As used herein, a vehicle 201 may be any type of wheeled vehicle, such as a car, a bus, or a rover. The vehicle 201 may also be an autonomous or semi-autonomous vehicle. For example, some embodiments control the motion of the vehicle 201. An example of motion includes the lateral motion of the vehicle, which is controlled by a steering system 203 of the vehicle 201. In one embodiment, the steering system 203 is controlled by the controller 202. Additionally or alternatively, the steering system 203 may be controllable by a driver of the vehicle 201.

[0058] The vehicle may also include an engine 206 that can be controlled by the controller 202 or by other components of the vehicle 201. The vehicle may also include one or more sensors 204 for sensing the surrounding environment in traffic situations, including, for example, other vehicles, traffic signs, traffic lights, lane boundaries, and / or road curvature. Examples of sensors 204 include a distance rangefinder, radar, lidar, and a camera. The vehicle 201 may also include one or more sensors 205 for sensing its current momentum and internal status. Examples of sensors 205 include a global positioning system (GPS), an accelerometer, an inertial measurement unit, a gyroscope, a shaft rotation sensor, a torque sensor, a deflection sensor, a pressure sensor, and a flow sensor. The sensors provide information to the controller 202. The vehicle may also include a transceiver 206 that enables communication functions of the controller 202 through wired or wireless communication channels.

[0059] 2B shows a schematic diagram of the interaction between controller 202 (i.e., a mixed integer predictive controller) and other controllers 220 of vehicle 201, according to some embodiments. In some embodiments of the invention, other controller 220 includes vehicle control module 163 and / or actuator control module 164, as shown in FIG. 1D. For example, in some embodiments of the invention, controller 220 of vehicle 201 is steering controller 225 and brake / throttle controller 230, which control the rotation and acceleration of vehicle 201, respectively. In such a case, mixed integer predictive controller 202 outputs control inputs to controllers 225 and 230 to control the state of vehicle 201.

[0060] In some embodiments of the invention, controller 220 includes a high-level controller, for example, a lane keeping assist controller 235 and / or a reference trajectory tracking controller 240, which further processes the control inputs of mixed integer predictive controller 202. In some embodiments of the invention, controller 220 uses the output of mixed integer predictive controller 202 to control at least one actuator of vehicle 201, such as the steering and / or brakes of vehicle 201, to control the motion of vehicle 201. Furthermore, mixed integer predictive controller 202 determines inputs to vehicle 201 based on the mixed integer control solution, where the inputs to vehicle 201 include one or a combination of acceleration of vehicle 201, engine torque, brake torque, and steering angle of vehicle 201, and the discrete optimization variables model one or a combination of discrete control decisions, system dynamics switches, gear shifts, lane change commands, and obstacle avoidance constraints.

[0061] In some embodiments of the present invention, the reference trajectory tracking controller 240 is implemented using a linear, linear time-varying, or nonlinear MPC controller to solve a convex quadratic programming (QP) problem or a nonconvex nonlinear programming (NLP) problem at each sampling period and calculate a series of vehicle actuation commands to achieve an actual trajectory for the vehicle as close as possible to the motion trajectory calculated by the mixed-integer predictive controller 202. Some embodiments of the present invention are based on the recognition that the NLP can be solved using a sequential convex programming (SCP) technique or a sequential quadratic programming (SQP) technique, and that each convex QP subproblem can be solved using a convex optimization algorithm, such as an active set solver, an interior point algorithm, a projected gradient method, a nonnegative least squares solver, or an alternating direction method of multipliers (ADMM).

[0062] 2C shows a schematic diagram of a path and / or motion planning method for a controlled vehicle employing principles of some embodiments. Additionally, FIG. 2C shows a schematic diagram of a controlled autonomous or semi-autonomous vehicle 250 for which a dynamically feasible and often optimal trajectory 255 can be calculated using embodiments of the present disclosure. The generated trajectory aims to keep the vehicle within a particular road boundary 252 and to avoid other controlled and / or uncontrolled vehicles, i.e., those vehicles are obstacles 251 to this particular controlled vehicle 250. In some embodiments, each of the obstacles 251 can be represented by one or more inequality constraints in a time or space formulation of a constrained mixed-integer programming problem, including one or more additional discrete variables for each obstacle. For example, based on an embodiment configured to implement a mixed integer model predictive controller, the controlled autonomous or semi-autonomous vehicle 250 can make discrete decisions in real time, such as determining whether to change lanes, stay in lane, or stop, or to pass another vehicle on the left or right, or alternatively stay behind another vehicle within the current lane of road 252, while also making continuous decisions in real time, such as speed, acceleration, or steering inputs to control the motion of vehicle 250.

[0063] 2D illustrates an exemplary traffic scene for a single-vehicle or multi-vehicle decision-making module according to some embodiments. FIG. 2D illustrates a situation with traffic consisting of one or more controlled vehicles, referred to as ego-vehicle 271, other vehicles designated similarly as 272, a lane 273 designated, for example, as L6, a stop lane 274 designated, for example, as S1, and an intersection 275 designated, for example, as I3. For a vehicle at location 261 with final destination 262, routing module 161 provides a series of roads designated, for example, as arrow 263, and a series of turns designated, for example, as arrow 264. Note, however, that the series of roads 263 and the series of turns 264 do not themselves specify the vehicle's trajectory or path. There are multiple discrete decisions to make, such as which lane to drive the vehicle in, whether the vehicle should change lanes or remain in its current lane, whether the vehicle should begin slowing down to stop in a stop lane, whether the vehicle is allowed to cross an intersection, etc. Furthermore, there are multiple sequential decisions to be made, such as a timed series of positions and orientations that the vehicle must achieve in its journey from its starting point to its destination. These decisions are highly dependent on the current traffic at the moment the vehicle reaches the corresponding position, which is generally unknown to the routing module due to the uncertainty in traffic movement and the uncertainty of the moment the vehicle reaches its position. In some embodiments of the present disclosure, a motion plan for one or more controlled ego-vehicles 271 can be computed by solving one or more connected mixed integer programming problems, possibly using communications that enable vehicle-to-vehicle (V2V) and / or smart infrastructure system-to-vehicle (V2X) cooperation.

[0064] Some embodiments of the present invention are based on the recognition that, depending on the sensing infrastructure of a transportation network, obtaining accurate predictions of the behavior of one or more other vehicles, e.g., human-driven vehicles (HDVs) in the vicinity of a controlled vehicle 120 in a traffic environment, can prove challenging. Because of this recognition, in some embodiments of the present invention, a vehicle decision-making and motion planning system (DM-MPS) is implemented as a mixed-integer predictive controller 110 with a receding horizon approach, in which a constrained MIP is solved at each sampling time step based on the most recent information from sensors and communication devices 140. For example, in some embodiments of the present invention, the sampling period is 0.25-0.5 seconds long for the decision-making and motion planning module 162 and 0.025-0.05 seconds long for the vehicle control module 163. The inherent feedback mechanism of the receding horizon strategy for a vehicle control architecture, including at least the decision-making and motion planning module 162 and the vehicle control module 163, can reconcile any discrepancies in predictions of the behavior of one or more other vehicles.

[0065] 3A shows a block diagram of a conversion from a real-world representation to an approximate representation of a constrained optimization problem in a vehicle decision-making and motion planning system for a controlled vehicle, according to some embodiments of the present invention. Some embodiments recognize that due to the complexity of real-world traffic conditions 305 in a road map environment 310 and the need for the controlled vehicle 120 to satisfy complex traffic rules 311, constrained optimization of simultaneous decision-making and motion planning for a vehicle traveling on a road requires formulating 315 and solving a mixed-integer non-convex constrained optimization problem given the controlled vehicle's current state 301. Unfortunately, the geometric design of the road map environment 310, defined by one or a combination of road alignment, profile, and cross-section, the traffic on the road formed by the vehicle and other vehicles and pedestrians in the real-world traffic conditions 305, and the complex traffic rules 311 on the road that restrict the vehicle's operation, form a very complex vehicle decision-making and motion planning problem.

[0066] In some embodiments of the present invention, a transformation and solution of a mixed integer convex programming (MICP) problem is instead used to calculate 320 an approximation to an optimal motion trajectory and optimal sequence of actions for the controlled vehicle, such that an approximation to the mixed integer non-convex constrained optimization problem 315 can be calculated in a computationally efficient manner. According to some embodiments of the present invention, the MICP solution 320 includes a reference motion trajectory that is used by the vehicle control system 325 to cause the controlled vehicle 120 to achieve an actual trajectory as close as possible to the motion trajectory provided by the decision-making and motion planning module as illustrated in FIG. 1D .

[0067] In some embodiments of the invention, examples of mixed-integer non-convex constrained optimization problems include nonlinear vehicle dynamics and / or nonlinear road boundary constraints in combination with mixed-integer equality and / or inequality constraints, enforcing traffic rules such as, for example, collision avoidance constraints, lane change constraints, and / or traffic intersection stopping constraints. Examples of real-world traffic situations for vehicle decision-making and motion planning may include (semi-)autonomous vehicles and one or more other vehicles traveling in a complex environment consisting of one or more connected road segments, with one or more lanes, one or more speed zones, one or more traffic intersections, one or more stopping zones, one or more traffic lights, and / or one or more merging points.

[0068] In some embodiments of the present invention, the proposed vehicle decision-making and motion planning system is realized using a predetermined traffic situation that results in a mixed-integer convex optimization problem, where a transformation and solution procedure 320 transforms one or more parameters of the current real-world traffic situation into one or more parameters of the predetermined traffic situation, and the resulting MICP forms an approximate representation of the constrained optimization problem in the vehicle decision-making and motion planning system for the real-world traffic situation of the controlled vehicle according to some embodiments of the present invention. For example, in some embodiments of the present invention, the predetermined traffic situation includes a controlled vehicle on a straight road segment having one or more lanes using a road-aligned coordinate system, predicted motion trajectories for one or more other vehicles and / or traffic parties, one or more obstacle avoidance constraints that the controlled vehicle stays outside a safety region around the predicted motion trajectories of the one or more other vehicles and / or traffic parties, one or more conflict zone constraints for each traffic intersection and / or merge point, and one or more spatially dependent zone constraints along the controlled vehicle's future planned route within the transportation network.

[0069] FIG. 3B shows a block diagram of a transformation and solving 320 strategy for computing an approximation of an optimal motion trajectory and optimal sequence of actions for a controlled vehicle, including a first transformation step 330 to compute a mixed-integer convex approximation of the vehicle decision-making and motion planning problem, a solving step 340 of the MICP problem to compute an approximate representation of the optimal motion trajectory and sequence of control actions, followed by a second back-transform step 345 from the approximate representation of the optimal motion trajectory and sequence of control actions to a real-world representation of the traffic situation.

[0070] For example, in some embodiments of the present invention, the first transformation step 330 includes transforming the vehicle and its dynamically changing environment from a real-world coordinate system to a road-aligned coordinate system to reduce the computational cost of the DM-MPS system at each sampling instant. Some embodiments of the present invention are based on the recognition that the first transformation step 330 needs to be applied to all components within the complex traffic situation 305, including, for example, the road map environment 310, the current and future predicted positions of other traffic parties, and complex traffic rules 311, such as collision avoidance, lane change behavior, speed zones, traffic intersection and traffic light behavior, stopping zone behavior, and / or safety constraints on merging points. In some embodiments of the present invention, the first transformation step 330 of the vehicle prediction model results in updated boundary constraints, for example, updated limits on the steering and / or lateral speed of the controlled vehicle 120 to take into account the curvature of road segments along the route of the controlled vehicle 120.

[0071] Some embodiments of the present invention are based on the recognition that real-world safety and traffic rules can be satisfied by the DM-MPS by using a first transformation step to compute 330 a mixed-integer convex approximation of the vehicle decision-making and motion planning problem that includes each of the traffic parties and their predictive models, and includes a convex approximation of each of the traffic rules using a road-aligned coordinate system. In some embodiments of the present invention, the convex approximation of each of the traffic rules is used in the first transformation step 330 to conservatively satisfy each of the safety and traffic rules in the real-world representation 345 of the traffic situation.

[0072] Some embodiments of the present invention are based on the recognition that the first transformation step 330 enables simultaneous computation of discrete decisions and continuous actions in the DM-MPS system, which can be formulated as a structured mixed-integer linear programming (MILP), structured mixed-integer quadratic programming (MIQP), or structured mixed-integer quadratically constrained quadratic programming (MIQCQP) problem that can be solved efficiently, for example, using branch-and-bound optimization techniques. In some embodiments of the present invention, after computing 340 a solution to the MILP at each sampling instant of the DM-MPS system, a second inverse transformation step 345 is used to back-transform from the approximate representation of the optimal motion trajectory and sequence of control actions using a road-aligned coordinate system to a real-world representation of the optimal motion trajectory, which can be executed by the vehicle control system 325. In some embodiments of the present invention, the vehicle control system is implemented using a model predictive controller (MPC), which aims to follow the continuous reference trajectory computed by the DM-MPS module.

[0073] 3C shows a block diagram of a transformation and solution strategy 360 for computing an approximation of an optimal motion trajectory and an optimal sequence of actions for a controlled vehicle, according to some embodiments of the present invention, for vehicle decision-making and motion planning, including relaxing 355 one or more configuration parameters that cause non-convexity of the constrained optimization problem, and restricting 356 one or more constraint parameters that are independent of the non-convexity of the constrained optimization problem. In some embodiments of the present invention, the vehicle decision-making and motion planning system is based on formulating and solving 360 the MICP problem, which computes an approximate representation of the optimal motion trajectory and sequence of control actions, including the relaxed configuration parameters 355 and restricted constraint parameter values ​​356, followed by a second inverse transformation step 345 from the approximate representation of the optimal motion trajectory and sequence of control actions to a real-world representation of the traffic situation.

[0074] Some embodiments are based on the recognition that each real-world traffic situation can be represented by a set of one or more parameters (350). Some of the parameters, such as road curvature or vehicle shape, can cause non-convexity of the constrained optimization problem for vehicle decision-making and motion planning. Other parameters, such as constraints on longitudinal velocity and / or acceleration and constraints on lateral velocity and / or acceleration, are independent of the non-convexity, i.e., do not cause non-convexity of the optimization problem. For example, some embodiments of the present invention use a convex approximation of a non-linear vehicle kinematics model that includes a constraint on lateral velocity that depends on longitudinal velocity to avoid non-convexity of the optimization problem.

[0075] Some embodiments are based on the recognition that one or more configuration parameters that cause non-convexity can be relaxed 355 at the expense of restricting 356 one or more constraint parameters that are independent of the non-convexity. For example, in some embodiments of the present invention, a configuration parameter for road curvature can be relaxed to straighten the road by restricting constraints on the lateral speed and / or lateral acceleration of a vehicle. In another example, in some embodiments of the present invention, a parameter for left or right turns at a traffic intersection can be relaxed to straighten the road by restricting constraints on the lateral position of a vehicle and / or by restricting constraints on the lateral speed and / or lateral acceleration of a vehicle. Similarly, a parameter for vehicle steering speed constraints can be relaxed to linearize the vehicle model by restricting constraints on the lateral speed and / or lateral acceleration of a vehicle. As yet another example, in some embodiments of the present invention, a parameter for the physical shape of a vehicle can be relaxed by restricting constraints on the position of the vehicle due to collision avoidance constraints. In this way, the mixed integer non-convex constrained optimization problem can be transformed into a mixed integer convex constrained optimization problem by relaxing parameters of the current real-world traffic situation that cause the non-convexity of the mixed integer non-convex constrained optimization problem, and by regulating at least some parameters of the current real-world traffic situation that are independent of the non-convexity of the mixed integer non-convex constrained optimization problem.

[0076] In some embodiments of the present invention, the proposed system and method for vehicle decision-making and motion planning uses, at each control time step, a transformation from a set of one or more parameters 350 in a real-world traffic situation to a relaxation 355 of configuration parameters and a restriction 356 of one or more constraint parameters in a mixed-integer convex programming (MICP) approximate representation 360 of the real-world traffic situation. In some embodiments of the present invention, the transformation of the real-world traffic situation of a vehicle motion planning problem involving one or more other vehicles on a curved road segment leads to an approximate representation of the vehicle motion planning problem involving one or more other vehicles on a straight road segment with restricted constraint parameter values, e.g., restricted boundary values ​​of one or more constraint functions in a MICP-based approximate representation of the traffic situation involving a (piecewise-linear) dynamic model of the (semi-)autonomous vehicle and a simplified representation of the vehicle's environment in the traffic situation. The MICP solution 360 includes an optimal motion trajectory and a sequence of control actions for the vehicle, which is further transformed 345 from the approximate representation of the traffic situation back to the real-world representation, according to some embodiments of the present invention, to control the motion of the (semi-)autonomous vehicle in the real-world traffic situation.

[0077] Some embodiments of the present invention are based on the recognition that the MICP problem is a convex constrained optimization problem that is computationally inexpensive to solve for a fixed set of values ​​for each of the integer variables, and therefore computationally efficient branch-and-bound and / or machine learning based techniques can be used to efficiently compute the fixed set of values ​​for each of the optimization variables in the optimal MICP solution.

[0078] 4A illustrates a transformation 415 of a vehicle decision-making and motion planning problem in an exemplary traffic situation with one or more other vehicles on a curved road segment 410 from a real-world representation 400 to an approximate representation 420 of the vehicle motion planning problem with one or more other vehicles on a straight road segment 430 with restricted parameter values ​​regulated in a constrained mixed-integer convex optimization problem, in accordance with some embodiments of the present invention. More specifically, the real-world traffic situation 400 includes a curved road segment 410 having one or more lanes, a controlled vehicle 401, and one or more other vehicles 405. In some embodiments of the present invention, a predicted motion trajectory 406 may be calculated for each of the other vehicles from the current time step 405 to one or more future time steps 407 in a prediction time horizon.

[0079] In some embodiments of the present invention, the predicted motion trajectory 406 is calculated using a closed-loop kinematic vehicle model for lane keeping based on the modeling assumption that each of the other vehicles will remain in the current lane during the prediction time horizon. Some embodiments of the present invention are based on the recognition that approximation errors in the prediction model for the other vehicles can be corrected based on a receding horizon implementation of the vehicle decision-making and motion planning system. In some embodiments of the present invention, the predicted motion trajectory 406 is calculated using a data-based vehicle model aimed at predicting human driver behavior using, for example, deep learning, support vector machines, neural networks, neural processes, Gaussian processes, and other machine learning algorithms, or any combination of such deterministic and / or probabilistic prediction models. The data-based vehicle model can include both lane keeping, cruise control, stopping, and / or lane change behavior modeling, according to some embodiments of the present invention.

[0080] In some embodiments of the invention, the predictive model for one or more other vehicles takes into account interactions between other vehicles, between the vehicle and the traffic environment (e.g., traffic lights at intersections, speed limits, etc.), and / or between the controlled vehicle and one or more other vehicles; for example, slowing down of one vehicle may cause one or more other vehicles to slow down, or a lane change by one vehicle may cause a reaction, e.g., slowing down by one or more other vehicles. In some embodiments of the invention, the reactive and / or interactive predictive model is realized using a switching dynamic system of equations that allows switching between different modes of behavior modeling in response to actions of other vehicles and / or changes in the traffic environment.

[0081] In some embodiments of the present invention, transformation step 415 computes an approximate representation 420 of a vehicle motion planning problem including a straight road segment 430 having one or more lanes, a controlled vehicle 421, and one or more other vehicles 425, and / or a predicted motion trajectory 426 may be computed for each of the other vehicles in approximate representation 420 from the current time step 425 to one or more future time steps 427 in the prediction time horizon. For example, in some embodiments of the present invention, transformation step 415 may be a linear or nonlinear transformation that computes a road alignment representation of a real-world traffic situation, i.e., approximating the vehicle decision-making and motion planning problem on the curved road segment 410 by solving a simplified vehicle decision-making and motion planning problem on the straight road segment 430 with restricted constraint parameters. Embodiments of the present invention are based on the recognition that the vehicle decision-making and motion planning problem in approximate representation 420 results in an MICP problem that can be solved in a computationally efficient manner to compute an optimal motion trajectory and sequence of control actions for the vehicle.

[0082] 4B illustrates inversely transforming 450 an optimal solution to a mixed-integer convex optimization problem with constrained parameter values ​​for an approximate representation 440 of a vehicle decision-making and motion planning problem in an exemplary traffic situation with one or more other vehicles into an approximate solution to a mixed-integer non-convex optimization problem for a real-world representation 460 on a curved road segment 410, in accordance with some embodiments of the present invention. In some embodiments of the present invention, the optimal solution to the MICP includes an optimal motion trajectory 445 and corresponding sequence of control actions for a controlled vehicle 421 from the current time step 421 to one or more future time steps 446 within the prediction time horizon, while avoiding any collision with predicted motion trajectories 426 for one or more other vehicles from the current time step 425 to one or more future time steps 427 within the prediction time horizon. For example, in some embodiments of the present invention, the optimal motion trajectory 445 includes a series of vehicle positions, a series of orientation values, a series of longitudinal velocity values, a series of acceleration and / or deceleration values, a series of engine torque and / or brake torque values, a series of steering values, and / or a series of one or more lane changes at future time steps to control the motion of the vehicle 421 in the approximate representation 440 of a real-world traffic situation.

[0083] In some embodiments of the present invention, an inverse transformation step 450 is used to calculate an approximate motion trajectory 465 and a series of control actions for the vehicle 401 from the current time step 401 to one or more future time steps 466 within the prediction time horizon, while avoiding any collision with predicted motion trajectories 406 from the current time step 405 to one or more future time steps 407 within the prediction time horizon for one or more other vehicles, in a real-world representation 460 of a traffic situation. Some embodiments of the present invention are based on the recognition that the inverse transformation step 450 is used to calculate the approximate motion trajectory 465 and a corresponding series of control actions to control the motion of the (semi-)autonomous vehicle 401 in a real-world traffic situation. For example, in some embodiments of the present invention, the approximate real-world motion trajectory 465 includes a series of vehicle positions at future time steps, a series of orientation values, a series of longitudinal velocity values, a series of acceleration and / or deceleration values, a series of engine torque and / or braking torque, a series of steering values, and / or a series of one or more lane changes to control the motion of the vehicle 401 in the real-world traffic situation 460.

[0084] In some embodiments of the present invention, a vehicle decision-making and motion planning system includes motion prediction for one or more human-driven vehicles (HDVs) using a switching dynamics system to represent the HDV's response to potentially changing traffic rules. For example, the following state-dependent switching dynamics can be used for HDV stopping at a traffic light at a particular intersection if the traffic light for the HDV's desired traffic direction is red and the HDV is within a predetermined distance from the stopping zone at that intersection: Otherwise, the HDV follows the leading vehicle and maintains a safe following distance if the leading vehicle is within a certain predetermined distance ahead of the HDV in the transportation network. Otherwise, the HDV travels at a desired target speed in traffic conditions.

[0085] For example, a switching dynamic system of equations for calculating predicted motion trajectories for one or more other vehicles in a traffic situation may be realized using one or more optimization variables and one or more mixed integer inequalities and / or equality constraints in the MICP problem solved by the proposed vehicle decision-making and motion planning system.

[0086] 5 illustrates a conversion step 515 from a real-world representation 500 of a vehicle decision-making and motion planning problem in an exemplary traffic situation with one or more other vehicles in the vicinity of a traffic intersection to an approximate representation 520 of a vehicle motion planning problem with one or more other vehicles on a straight road segment with constrained limit parameter values, one or more exclusion zone constraints, and one or more collision avoidance constraints in a mixed-integer convex optimization problem, according to some embodiments of the present invention. In addition, FIG. 5 illustrates a back conversion 540 of the optimal solution to the mixed-integer convex optimization problem with constrained limit parameter values ​​for the approximate representation 520 of the vehicle decision-making and motion planning problem in an exemplary traffic situation with one or more other vehicles to an approximate solution of a mixed-integer non-convex optimization problem for the real-world representation 500.

[0087] In some embodiments of the present invention, a real-world traffic situation 500 includes a traffic intersection connecting multiple road segments and one or more lanes within each road segment, a controlled vehicle 501, one or more other vehicles and / or traffic parties 505a-505d, and multiple traffic rules determined by one or more traffic light signals 510a-510c that enable the vehicle to navigate through the traffic intersection in one or more crossing directions to avoid collisions, minimize congestion, and increase traffic flow at each time step. Some embodiments of the present invention are based on the recognition that the proposed decision-making and motion planning system (DM-MPS) uses a transformation step 515 to convert the complex real-world traffic situation 500 into an approximate representation 520 of a local neighborhood of a transportation network along a particular route of the controlled vehicle 501. For example, in some embodiments of the present invention, the route includes a series of road segments, a series of lanes, a series of turns, and / or a series of crossing directions through one or more traffic intersections in the transportation network that the vehicle 501 plans to traverse to reach its desired destination from its current location. This route can be provided by the user or calculated by the car navigation system or a route planning module.

[0088] In some embodiments of the present invention, the approximate representation 520 of the vehicle motion planning problem with regulated constraint parameter values ​​includes one or more zones, e.g., a first zone having only one lane defined based on an exclusion zone constraint 535, followed by a second zone having two lanes defined based on a different exclusion zone constraint 536, and potentially one or more additional zones including one or more mixed-integer inequality and / or equality constraints for enforcing traffic rules, e.g., a speed limit constraint, a lane change constraint, a stopping zone constraint, etc. Additionally, in some embodiments of the present invention, the approximate representation 520 includes the controlled vehicle 521, one or more other vehicles 525a-525d, and / or a predicted motion trajectory may be calculated for each of the other vehicles and / or traffic parties in the approximate representation 520 from the current time step to one or more future time steps in the prediction time horizon.

[0089] In some embodiments of the present invention, approximate representation 520 includes additional conflict zones 531 with one or more mixed-integer convex inequality and / or equality constraints that enforce rules at traffic intersections and / or merges. For example, in some embodiments of the present invention, traffic light signals 510a-510c in a real-world traffic situation are realized in approximate representation 530 by enforcing collision avoidance constraints on conflict zones 531 at one or more future time steps in a prediction time horizon of a vehicle decision-making and motion planning system. For example, if traffic light 530 is red for a particular crossing direction at a particular time step, one or more conflict zone constraints 531 force controlled vehicle 521 to stop before the traffic intersection in approximate representation 520, which corresponds to a stopping maneuver in real-world traffic situation 500. Additionally, if traffic light 530 is predicted to turn green at a future time step within the prediction time horizon, one or more conflict zone constraints 531 are removed from the constrained mixed-integer convex optimization problem at a future time step within the prediction time horizon to calculate an optimal motion plan for controlled vehicle 521 to cross the traffic intersection.

[0090] In some embodiments of the present invention, the optimal solution to the MICP includes an optimal movement trajectory 522 and a corresponding set of control actions for the controlled vehicle 521 from the current time step 521 to one or more future time steps 522 within the prediction time horizon, so as to avoid any collisions with the predicted movement trajectory for one or more other vehicles 525a-525d, avoid any violation of the conflict zone constraint 531, and enforce complex traffic rules in a local neighborhood along a particular route of the controlled vehicle 501 within the transportation network, while taking into account one or more additional zone constraints 535 and / or 536. In some embodiments of the invention, the inverse transformation step 540 is used to calculate an approximate motion trajectory 545 and a series of actions to safely and optimally control the motion of the (semi-)autonomous vehicle 501 in the real-world traffic situation 500 from the current time step to one or more future time steps in the prediction time horizon, while avoiding any collision with the predicted motion trajectories of one or more other vehicles 505a-505d in the real-world traffic situation 500, and while avoiding any violation of complex traffic rules.

[0091] For example, in some embodiments of the present invention, the optimal motion trajectory 522 and / or the transformed motion trajectory 545 include a series of vehicle positions, a series of orientation values, a series of longitudinal velocity values, a series of acceleration and / or deceleration values, a series of engine torque and / or braking torque values, a series of steering values, and / or a series of one or more lane changes at future time steps to control the motion of the vehicle 501 in the real-world traffic situation 500.

[0092] 6A shows a block diagram of a system and method for mixed-integer model predictive control (MI-MPC) to implement a predictive controller 110 that calculates control signals 111 given the current state and control commands 101 of the system 121 in a proposed vehicle decision-making and motion planning system, according to some embodiments. Specifically, MI-MPC calculates a control solution, e.g., a solution vector 635 including a set of future optimal discrete and continuous control inputs over the system's forecast time horizon, by solving a constrained mixed-integer convex programming (MICP) problem 630 at each control time step. The MICP data 625 of the objective function, equality constraints, and discrete and continuous inequality constraints in this optimization problem 630 depend on dynamic models of the controlled vehicle and traffic environment, system limits, and one or more traffic rules 620 in an approximate representation of the traffic situation, the current state of the system 121, the control objective, and the control commands 101.

[0093] In some embodiments, solving this inequality-constrained mixed-integer convex optimization problem 630 uses state and control values ​​over the prediction time horizon from a previous control time step, which can be read from memory (610). This concept is called a warm or hot start of the optimization algorithm and can reduce the required computational effort of the MI-MPC controller in some embodiments of the invention. Similarly, the corresponding solution vector 635 can be used to update and store (640) a set of optimal state and control values ​​for the next control time step.

[0094] In some embodiments, the mixed integer optimization algorithm is based on a search algorithm, and the MI-MPC controller updates and stores additional mixed integer programming problem solution information to reduce the computational effort of the search algorithm at the next control time step (640). In one embodiment, the MI-MPC problem is solved at each control time step using a branch-and-bound optimization method, and the warm-start information 640 includes data related to nodes in a binary search tree that are part of the solution path from the root node to the leaf node where the optimal integer feasible control solution is found, to improve node selection and variable branching strategies from one control time step to the next.

[0095] 6B shows a block diagram of an MI-MPC method for solving an optimal control structured mixed-integer linear-quadratic optimization problem 650 to calculate control signals 111 at each control time step and a motion plan including a series of future discrete decisions and continuous actions for the controlled vehicle, given the current state 121 and commands 101 of the vehicle 120. Some embodiments of the present invention are based on a linear dynamic model of the vehicle and / or a linear dynamic model of the traffic environment 653 with one or more linear mixed-integer equality constraints 652, one or more linear mixed-integer inequality constraints 654, one or more linear discrete equality and / or integer feasibility constraints 655, one or more linear terminal inequality constraints 656, and a linear-quadratic convex objective function 651, where one or more constrained mixed-integer convex quadratic programming (MIQP) problems 650 need to be solved at each control time step of the proposed vehicle decision-making and motion planning system. In that case, the MIQP data 625 includes Hessian and constraint Jacobian matrices 646 and corresponding gradient and constraint evaluation vectors 647. Generally, the linear discrete equations and / or integer feasibility constraints 655 are linear functions of the state and control values.

number

[0096] In some embodiments of the present invention, the linear discrete equality constraints 655 are binary equality constraints

number

[0097]

number

[0098]

number

[0099]

number

[0100]

number

[0101]

number

number

[0102]

number

[0103]

number

[0104] In some embodiments of the present invention, a linear dynamics model of a controlled vehicle in an approximate representation of a real-world traffic situation, including both simplified vehicle dynamics and additional linear dynamics for enforcing traffic rules, e.g., related to lane changes, may be read as follows:

number

[0105]

number

[0106]

number

[0107]

number

[0108] Some embodiments of the present invention are based on the recognition that MICP 630 includes one or more obstacle avoidance constraints that are generally time-varying and can be used to implement collision avoidance of the controlled vehicle against other stationary or dynamic vehicles, bicycles, or pedestrians, but can also be used to implement traffic rules at stop signs or traffic lights at traffic intersections along a particular route of the controlled vehicle within a transportation network. The latter allows the same MICP formulation to be used to model approximate representations of many different real-world traffic situations.

[0109]

number

[0110]

number

[0111] 7B illustrates one or more obstacle avoidance constraints used in the proposed vehicle decision-making and motion planning system for a controlled vehicle 721 in an approximate representation of an exemplary real-world traffic situation involving one or more other vehicles 725a-725b. For example, in some embodiments of the present invention, the one or more obstacle avoidance constraints constrain the controlled vehicle 721 to be outside a safety region 730a around vehicle 725a and outside a safety region 730b around vehicle 725b. In some embodiments of the present invention, based on the vehicle's predicted motion trajectory 726 from a current time step 725a to one or more future time steps 728, the controlled vehicle 721 can be constrained to be outside the safety region 730a at the current time step, and the vehicle 721 can be constrained to be outside one or more other safety regions 732 around the predicted position and orientation of the vehicle 728 at one or more future time steps in the vehicle decision-making and motion planning system's prediction time horizon. Some embodiments of the present invention are based on the recognition that obstacle avoidance constraints that lie outside a safe region around the predicted motion trajectory for each of the obstacles lead to a set of time-varying mixed-integer inequality constraints in MICP 630 that are solved at each control time step.

[0112] 7C illustrates one or more obstacle avoidance constraints used in the proposed vehicle decision-making and motion planning system in an approximate representation of an exemplary real-world traffic situation for a controlled vehicle 721, with one or more other vehicles 725a-725b, with corresponding safety areas 730a-730b, and with one or more traffic intersections 740 along a particular route of the controlled vehicle 721 in a transportation network. Some embodiments of the present invention are based on the recognition that a set of mixed-integer inequality constraints can be used to enforce the controlled vehicle 721 to stay outside the safety area 745 around each traffic intersection 740 at a particular time step within the prediction time horizon of the vehicle decision-making and motion planning system.

[0113] For example, if a traffic light is red for a desired crossing direction at the current time step, one or more collision avoidance constraints force controlled vehicle 721 to stop before entering safety zone 745 around traffic intersection 740 in an approximate representation of the traffic situation, which corresponds to a stopping maneuver in a real-world traffic situation. In some embodiments of the invention, the collision avoidance constraint on safety zone 745 around traffic intersection 740 is referred to as a conflict zone constraint. For example, if a traffic light is predicted to turn green for a desired crossing direction at a future time step within the prediction time horizon, then one or more conflict zone constraints are relaxed and / or removed from the constrained mixed-integer convex optimization problem at one or more future time steps of the prediction time horizon to calculate an optimal motion plan for controlled vehicle 721 to cross traffic intersection 740 in the desired crossing direction.

[0114] In some embodiments of the present invention, complex traffic rules regarding priority at which vehicles may enter traffic intersections without traffic lights may be implemented in a similar manner, e.g., by enforcing one or more conflict zone constraints at certain time steps within the prediction time horizon and relaxing and / or removing conflict zone constraints at other time steps within the prediction time horizon. For example, in some embodiments of the present invention, controlled vehicle 721 is permitted to enter safety area 745 around traffic intersection 740 only after coming to a complete stop inside a stopping zone ahead of the traffic intersection and only when there are no other vehicles crossing the traffic intersection in an intersecting direction that could lead to a potential collision.

[0115] 7D illustrates one or more spatially dependent zone constraints 761-762 that are used in the proposed vehicle decision-making and motion planning system in an approximate representation of an exemplary real-world traffic situation for a controlled vehicle 751 with one or more other vehicles 752-753. Some embodiments of the present invention are based on the recognition that traffic rules change when the controlled vehicle 751 transitions from one spatially dependent zone 761 to another spatially dependent zone 762. For example, in some embodiments of the present invention, the spatially dependent zone constraints include:

[0116] A limit on the maximum number of lane changes allowed for a controlled vehicle 751 within a particular spatially dependent zone, for example, no lane changes are allowed while traversing a traffic intersection.

[0117] Constraints on lanes that the controlled vehicle 751 is allowed to travel in within a particular spatially dependent zone of a road segment, for example, the vehicle must merge into a lane before a particular point along the controlled vehicle's 751 route in the transportation network.

[0118] A speed limit constraint that is spatially dependent and zone dependent, for example, the speed limit value generally decreases as the controlled vehicle 751 enters a congested urban neighborhood.

[0119]

number

[0120]

number

[0121]

number

[0122] 8A shows a schematic diagram of an example binary control-variable search tree representing a tree of nested search spaces for integer-feasible control solutions, according to some embodiments. FIG. 8A shows a schematic diagram of a branch-and-bound method used to implement an MI-MPC controller in some embodiments by showing a binary search tree 800 in a particular iteration of a mixed-integer optimization algorithm. The main concept of the branch-and-bound (B&B) method is to sequentially create partitions of the original problem and then attempt to solve those partitions, each corresponding to a particular region of the discrete control-variable search space. In some embodiments, the branch-and-bound method selects a partition, or node, and selects discrete control variables to branch this partition into smaller partitions or search spaces, resulting in a tree of nested partitions or search spaces.

[0123]

number

[0124]

number

[0125] FIG. 8B illustrates a block diagram of a branch-and-bound mixed-integer optimization algorithm for searching for an integer-feasible optimal solution based on a tree of nested search spaces and corresponding lower / upper bounds, according to some embodiments. The block diagram of the branch-and-bound mixed-integer optimization algorithm illustrated in FIG. 8B can be used to solve MICP problems in some embodiments of the present invention. The branch-and-bound method initializes branch search tree information for a mixed-integer quadratic program (MIQP) at the current control time step (810) based on MIQP data 625 consisting of an MIQP matrix 646 and an MIQP vector 647. The initialization further generates a warm-start initialization for the current control time step (810) using the branch search tree information and MIQP solution information 610 from the previous control time step. The main goal of the optimization algorithm is to construct lower and upper bounds for the objective value of the mixed-integer control solution. In step 811, if the gap between the lower and upper bounds is smaller than a certain tolerance, a mixed-integer optimal control solution 635 is found.

[0126] Some embodiments of the present invention recognize that as long as the gap between the lower and upper bounds in step 811 is greater than a certain tolerance and the optimization algorithm has not yet reached its maximum run time, the branch and bound method continues to iteratively search for a mixed-integer optimal control solution (635). Each iteration of the branch and bound method begins by selecting the next node in the tree, corresponding to the next region or partition of the integer variable search space, with possible variable fixes based on a pre-solution branching technique (815). After node selection, the corresponding integer relaxed MPC problem is solved, with possible variable fixes based on a post-solution branching technique (820).

[0127] Some embodiments of the present invention are based on the recognition that if an integer-relaxed MPC problem has a feasible solution, the resulting relaxed control solution provides a lower bound on the objective value for that particular region or partition of the integer variable search space. If, in step 821, it is determined that the objective is greater than the currently known upper bound on the objective value of the optimal mixed-integer control solution, then the selected node is pruned or removed from the branch tree (840). However, if, in step 821, it is determined that the objective is lower than the currently known upper bound and the relaxed control solution is integer-feasible (825), then, in step 830 of the branch-and-bound optimization algorithm, the currently known upper bound and the corresponding estimate of the mixed-integer control solution are updated.

[0128] Some embodiments of the present invention are based on the recognition that if an integer-relaxed MPC problem has a feasible solution and the objective is lower than the currently known upper bound (821), but the relaxed control solution is not yet integer-feasible, then the global lower bound of the objective can be updated (835) to the minimum of the objective values ​​of the remaining leaf nodes in the branching tree, and the selected node is pruned (840) from the tree. Additionally, starting from the current node, discrete variables with fractional values ​​are selected for branching according to a particular branching strategy (845), and the resulting subproblems corresponding to regions or partitions of the discrete search space are created and added as children of that node in the branching tree (850).

[0129]

number

[0130] Some embodiments are based on a branch-and-bound method using a depth-first node selection approach, which can be implemented using a last-in-first-out (LIFO) buffer. The next node to be solved is selected as one of the children of the current node, and this process is repeated until the node is pruned, i.e., becomes infeasible, optimal, or is dominated by a currently known upper bound, followed by a backtracking procedure. Alternatively, some embodiments are based on a branch-and-bound method using a best-first approach, which selects the node with the smallest current local lower bound. Some embodiments employ a combination of depth-first and best-first node selection approaches, where a depth-first node selection approach is used until an integer feasible control solution is found, and then a best-first node selection approach is used in subsequent iterations of the branch-and-bound-based optimization algorithm. The motivation for the latter implementation is to aim to find an integer feasible control solution early at the start of the branch-and-bound procedure (depth-first) to allow early pruning, followed by a more greedy search for better feasible solutions (best-first).

[0131] The branch and bound method continues iterating until one or more of the following conditions are met:

[0132] · The maximum processor execution time has been reached.

[0133] All nodes in the branch search tree were pruned so that no new nodes could be selected to solve a convex relaxation or branch.

[0134] The optimality gap between the global lower and upper bounds for the mixed integer control solution objective is smaller than the tolerance value.

[0135]

number

[0136]

number

[0137] Some embodiments of the present invention are based on the recognition that if MICP 970 can be formulated as an MILP, then CP 980 is a convex linear program (LP). Some embodiments of the present invention are based on the recognition that if MICP 970 can be formulated as an MIQP, then CP 980 is a convex quadratic program (QP). Some embodiments of the present invention are based on the recognition that if MICP 970 can be formulated as an MIQCQP, then CP 980 is a convex quadratically constrained quadratic program (QCQP). Some embodiments of the present invention are based on the recognition that if MICP 970 can be formulated as an MISOCP, then CP 980 is a convex quadratic cone program (SOCP).

[0138]

number

[0139] In some embodiments of the present invention, the predictive model 905 may be deterministic, including, for example, multi-layer perceptrons, convolutional neural networks (CNNs), recurrent neural networks (RNNs), transformers, kernel regressions, support vector machines, and other machine learning algorithms, or any combination of such deterministic predictive models. Alternatively, in some embodiments of the present invention, the predictive model may be probabilistic, including, for example, Bayesian neural networks, neural processes, Gaussian processes, Kriging interpolation, and other machine learning algorithms, or any combination of such deterministic and / or probabilistic predictive models.

[0140] Some embodiments of the present invention are based on the recognition that one or more permutation-invariant and / or permutation-equivant layers can be used in the predictive model 905 to improve the accuracy of the predictions and increase the likelihood of feasibility and / or optimality of the CP approximation 980. For example, some embodiments of the present invention may use a deep set architecture to ensure that the predictions 905 of a set of fixed values ​​of discrete variables are invariant to the order of parameters corresponding to one or more obstacles in a traffic environment. Alternatively, some embodiments of the present invention may use one or more equivariant deep set layers to ensure that a permutation in the order of parameters corresponding to one or more obstacles in a traffic environment results in the same permutation in the order of the predictions 905 of a set of fixed values ​​of discrete variables. Finally, some embodiments of the present invention may use one or more symmetric layers to enforce symmetric properties of the predictive model 905 with respect to symmetric transformations in a traffic environment for a vehicle decision-making and motion planning system.

[0141]

number

[0142]

number

[0143]

number

[0144] In some embodiments of the invention, the modification method 910 is an iterative pre-solution-based modification method based on an iterative procedure, where each iteration performs a single pre-solution step, after which one or more discrete optimization variables are fixed to a set of predicted fixed values ​​905. In some embodiments of the invention, the single pre-solution step itself consists of an iterative procedure of one or more pre-solution operations to reduce the number of discrete and / or continuous optimization variables, constrain the ranges of the optimization variables, and / or constrain the ranges of one or more inequality constraints. To ensure computational efficiency, the iterative procedure of the single pre-solution step then performs one or more iterations until the problem is detected as infeasible, until sufficient progress is not made between two successive iterations, or until a predetermined time limit is reached. Examples of pre-solution operations include domain propagation, range tightening, double fixing, implicit variable substitution, coefficient tightening, probing, and detection and removal of redundant variables and / or constraints. Additionally, each of the pre-solution operations can be performed on an individual variable, an individual constraint, multiple variables and / or multiple constraints.

[0145]

number

[0146] FIG. 9D shows a block diagram of MICP solving 630 for calculating a feasible, but potentially suboptimal, solution vector 635 that defines the control signal 111 based on: predicting 905 a set of fixed values ​​for the discrete optimization variables given the current problem parameter value θ 675, using this to fix 911 a first subset of the discrete variables in the MICP to the predicted values ​​output by the trained parametric function; then performing 912 a pre-solution-based modification to fix 910 the remaining subset of the discrete variables to values ​​that are uniquely defined by the fixed values ​​of the first subset of discrete variables and that are based on the constraints in the MICP; and then solving 980 an optimal control structured CP approximation to increase the probability of feasibility and optimality for the updated set of fixed values ​​of the discrete variables.

[0147]

number

[0148]

number

[0149]

number

[0150]

number

[0151] Some embodiments of the present invention are based on the recognition that the offline data generation and supervised learning procedure 1000 can be performed offline on a high-performance computer and therefore does not have stringent requirements on computational complexity and worst-case computation time, whereas the online variable fixing and optimal control solving procedure 1020 must be performed online with stringent requirements on computational complexity and worst-case computation time, and that online procedures are typically performed on embedded microprocessors with limited memory and computational power, for example using an embedded control unit (ECU) in a vehicle.

[0152]

number

[0153]

number

[0154]

number

[0155]

number

[0156]

number

[0157]

number

[0158]

number

[0159]

number

[0160]

number

[0161]

number

[0162]

number

[0163]

number

[0164] Embodiments of the present invention are based on the recognition that iterative pre-solution based correction methods can increase the probability of feasibility and / or optimality if the iterative correction methods remove or prune discrete variables of interest for which, for example, a machine learning based predictor provided erroneous predictions that would have resulted in an infeasible control solution.

[0165] The following description provides exemplary embodiments only and is not intended to limit the scope, applicability, or configuration of the present disclosure. Rather, the following description of exemplary embodiments provides those skilled in the art with an enabling description for implementing one or more exemplary embodiments. Contemplated are various changes that may be made in the function and arrangement of elements without departing from the spirit and scope of the disclosed subject matter as set forth in the appended claims.

[0166] In the following description, specific details are given for a thorough understanding of the embodiments. However, it will be understood by those skilled in the art that the embodiments may be practiced without these specific details. For example, systems, processes, and other elements in the disclosed subject matter may be shown as components in block diagram form so as not to obscure the embodiments in unnecessary detail. In other instances, well-known processes, structures, and techniques may be shown without unnecessary detail to avoid obscuring the embodiments. Furthermore, like reference numbers and names in the various drawings indicate like elements.

[0167] Also, particular embodiments may be described as a process that is depicted as a flowchart, a flow diagram, a data flow diagram, a structure diagram, or a block diagram. While a flowchart may describe operations as a sequential process, many of the operations can be performed in parallel or simultaneously. Additionally, the order of operations may be rearranged. A process may terminate when its operations are completed, but may have additional steps not discussed or included in the diagram. Moreover, not all operations in any specifically described process may occur in all embodiments. A process may correspond to a method, a function, a procedure, a subroutine, a subprogram, etc. When a process corresponds to a function, the termination of the function may correspond to a return of the function to the calling function or the main function.

[0168] Furthermore, embodiments of the disclosed subject matter may be implemented, at least in part, either manually or automatically. Manual or automatic implementations may be performed or at least assisted by machine, hardware, software, firmware, middleware, microcode, hardware description languages, or any combination thereof. When implemented in software, firmware, middleware, or microcode, the program code or code segments to perform the necessary tasks may be stored on a machine-readable medium. The necessary tasks may be performed by a processor.

[0169] The various methods or processes outlined herein may be coded as software executable on one or more processors using any one of a variety of operating systems or platforms. Additionally, such software may be written using any of a number of suitable programming languages ​​and / or programming or scripting tools, and may be compiled as executable machine language code or intermediate code that runs on a framework or virtual machine. Typically, the functionality of the program modules may be combined or distributed as desired in various embodiments.

[0170] The above-described embodiments of the present invention may be implemented in any of numerous ways. For example, embodiments may be implemented using hardware, software, or a combination thereof. If implemented in software, the software code may be executed on any suitable processor or collection of processors, whether provided on a single computer or distributed across multiple computers. Such a processor may be implemented as an integrated circuit with one or more processors within an integrated circuit component. However, a processor may be implemented using any suitable form of circuitry.

[0171] Embodiments of the present disclosure may be embodied as methods, for which examples are provided. The acts performed as part of the method may be ordered in any suitable manner. Thus, embodiments may be constructed in which acts are performed in an order different from that illustrated, which may include performing some acts shown as sequential acts in the exemplary embodiments simultaneously.

[0172] Although the present disclosure has been described with reference to certain preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the present disclosure. It is, therefore, within the scope of the claims to cover all such variations and modifications that are within the true spirit and scope of the present disclosure.

Claims

1. 1. A controller for controlling a vehicle traveling on a road, the road having a geometric design defined by one or a combination of an alignment, a profile, and a cross section of the road, different traffic situations being defined by different values ​​of parameters of a traffic rule for the geometric design of the road, traffic on the road, and the flow of traffic on the road, the controller comprising at least one processor and a memory having instructions stored thereon, the instructions, when executed by the at least one processor, causing the controller to: collecting parameters of a current real-world traffic situation for controlling the vehicle, the parameters including configuration parameters that cause non-convexity of a mixed-integer non-convex constrained optimization problem for simultaneous decision-making and motion planning of the vehicle, and constraint parameters that are independent of the non-convexity of the mixed-integer non-convex constrained optimization problem; converting the mixed integer non-convex constrained optimization problem for the current real-world traffic situation into a mixed integer convex optimization problem for an approximate representation of the real-world traffic situation by relaxing the configuration parameters and restricting corresponding constraint parameters; solving the transformed mixed integer convex optimization problem on the approximate representation of the real-world traffic situation to generate current control commands for controlling one or more actuators of the vehicle; a controller that causes the one or more actuators of the vehicle to be controlled in accordance with the control commands.

2. The controller of claim 1 , wherein the memory stores a predetermined traffic situation that results in the mixed integer convex optimization problem, and the controller transforms the parameters of the current traffic situation into the parameters of the predetermined traffic situation.

3. 2. The controller of claim 1, wherein the regulation of the limit parameters results in one or more updated boundary constraints in the mixed-integer convex optimization problem, for example, updated limits on steering and / or lateral speed of the controlled vehicle to account for curvature of one or more road segments along a route from the controlled vehicle's current position to a desired destination in a transportation network.

4. 3. The controller of claim 2, wherein the predetermined traffic situation includes one or a combination of: a predicted movement trajectory for the controlled vehicle on a straight road segment with one or more lanes and one or more other vehicles and / or traffic parties; one or more obstacle avoidance constraints for the controlled vehicle to stay outside a safety zone around the predicted movement trajectory of the one or more other vehicles and / or traffic parties; a lane change timing constraint; one or more conflict zone constraints for each traffic intersection and / or merge point; and one or more spatially dependent zone constraints along a future planned route of the controlled vehicle in a transportation network.

5. 5. The controller of claim 4, wherein the traffic rules for the current real-world traffic situation are implemented by the mixed-integer convex optimization problem for the given traffic situation using one or more mixed-integer convex equality and / or inequality constraints and one or more auxiliary discrete and / or continuous optimization variables.

6. 5. The controller of claim 4, wherein the predicted motion trajectories of the one or more other vehicles and / or traffic parties are calculated using a closed-loop kinematic vehicle model for lane keeping and / or adaptive cruise control, assuming that each of the other vehicles and / or traffic parties stays in a current lane during a prediction time horizon of the vehicle decision-making and motion planning system for the controlled vehicle.

7. 5. The controller of claim 4, wherein the predicted motion trajectories of the one or more other vehicles and / or traffic parties are calculated using a data-based vehicle model that aims to predict human driver behavior during a prediction time horizon of the vehicle decision-making and motion planning system for the controlled vehicle, the data-based vehicle model using one or a combination of deep learning, support vector machines, neural networks, neural processes, Gaussian processes, machine learning techniques, and probabilistic prediction models.

8. The controller of claim 1 , wherein the control commands are determined based on an inverse transformation of a solution of the mixed integer convex optimization problem to an approximate solution of the mixed integer non-convex optimization problem.

9. 9. The controller of claim 8, wherein the approximate solution of the mixed integer non-convex optimization problem includes a reference motion trajectory used by a vehicle tracking control system, the controlled vehicle achieving an actual motion trajectory provided by the decision-making and motion planning system that is as close as possible to the reference motion trajectory, and the vehicle tracking control system is implemented using a linear, linear time-varying, or nonlinear model predictive controller (MPC).

10. 2. The controller of claim 1, wherein the mixed integer convex optimization problem is a mixed integer linear programming (MILP) problem, a mixed integer quadratic programming (MIQP) problem, a mixed integer quadratically constrained quadratic programming (MIQCQP) problem, or a mixed integer quadratic cone programming (MISOCP) problem.

11. 2. The controller of claim 1, wherein a globally optimal or a feasible but suboptimal solution to the mixed integer convex programming (MICP) problem can be computed at each control time step using, for example, a branch and bound, branch and cut, branch and price, or any other variant of a tree search based optimization algorithm.

12. 2. The controller of claim 1, wherein a feasible but sub-optimal solution to the MICP problem is calculated at each control time step using a deep learning network architecture trained with machine learning to predict optimal values ​​for the MICP discrete and / or continuous optimization variables given current problem parameter values.

13. 13. The controller of claim 12, wherein the deep learning network architecture includes a permutation- or transformation-invariant layer that enforces that the output of the predictive model is invariant to permutations or transformations of one or more parameters of current real-world traffic conditions.

14. 13. The controller of claim 12, wherein the deep learning network architecture includes a permutation equivariant layer or a transformation equivariant layer that enforces that the output of the predictive model is equivariant to permutations or transformations of one or more parameters of the real-world traffic situation.

15. 13. The controller of claim 12, wherein the deep learning network architecture is evaluated one or more times to generate multiple predicted values ​​for each optimization variable in a set of optimization variables in the MICP to result in multiple candidate control solutions, and the controller performs a pre-solution-based refinement on each candidate control solution to convert the MICP into multiple convex programming (CP) problems and determine multiple reference motion trajectories and / or multiple sequences of control actions, and the controller is further configured to select a feasible motion trajectory for controlling the vehicle from the multiple motion trajectories.

16. 13. The controller of claim 12, wherein the deep learning network architecture comprises one or a combination of deterministic and probabilistic predictive models, wherein the deterministic model comprises one or a combination of a multi-layer perceptron, a convolutional neural network (CNN), a recurrent neural network (RNN), a transformer, a kernel regression, and a support vector machine, and wherein the probabilistic model comprises one or a combination of a Bayesian neural network, a neural process, a Gaussian process, and a Kriging interpolation.

17. 1. A method for controlling a vehicle traveling on a road, the road having a geometric design defined by one or a combination of an alignment, a profile, and a cross section of the road, different traffic situations being defined by different values ​​of parameters of a traffic rule for the geometric design of the road, traffic on the road, and the flow of traffic on the road, the method using a processor coupled to a memory having instructions stored thereon, the instructions, when executed by the processor, performing steps of the method, the steps comprising: collecting parameters of a current real-world traffic situation for controlling the vehicle, the parameters including configuration parameters that cause non-convexity of a mixed-integer non-convex constrained optimization problem for simultaneous decision-making and motion planning of the vehicle, and constraint parameters that are independent of the non-convexity of the mixed-integer non-convex constrained optimization problem; converting the mixed integer non-convex constrained optimization problem for the current real-world traffic situation into a mixed integer convex optimization problem for an approximate representation of the real-world traffic situation by relaxing the configuration parameters and restricting corresponding constraint parameters; solving the transformed mixed integer convex optimization problem on the approximate representation of the real-world traffic situation to generate current control commands for controlling one or more actuators of the vehicle; and controlling the one or more actuators of the vehicle in accordance with the control commands.

18. 18. The method of claim 17, wherein the memory stores a predetermined traffic situation that results in the mixed integer convex optimization problem, and the method transforms the parameters of the current traffic situation to the parameters of the predetermined traffic situation.

19. 18. The controller of claim 17, wherein the regulation of the limit parameters results in one or more updated boundary constraints in the mixed-integer convex optimization problem, for example, updated limits on steering and / or lateral speed of the controlled vehicle to account for curvature of one or more road segments along a route from the controlled vehicle's current position to a desired destination in a transportation network.

20. 1. A non-transitory computer-readable storage medium having embodied thereon a program executable by a processor for performing a method for controlling a vehicle traveling on a road, the road having a geometric design defined by one or a combination of an alignment, a profile, and a cross section of the road, different traffic situations being defined by different values ​​of parameters of traffic rules for the geometric design of the road, traffic on the road, and the flow of traffic on the road, the method comprising: collecting parameters of a current real-world traffic situation for controlling the vehicle, the parameters including configuration parameters that cause non-convexity of a mixed-integer non-convex constrained optimization problem for simultaneous decision-making and motion planning of the vehicle, and constraint parameters that are independent of the non-convexity of the mixed-integer non-convex constrained optimization problem; converting the mixed integer non-convex constrained optimization problem for the current real-world traffic situation into a mixed integer convex optimization problem for an approximate representation of the real-world traffic situation by relaxing the configuration parameters and restricting corresponding constraint parameters; solving the transformed mixed integer convex optimization problem on the approximate representation of the real-world traffic situation to generate current control commands for controlling one or more actuators of the vehicle; and controlling the one or more actuators of the vehicle according to the control commands.

Images