SYSTEM AND METHOD FOR VEHICLE DECISION AND MOTION PLANNING USING REAL-TIME MIXED integers

Abstract

A vehicle is controlled for travel on a road having a geometric design defined by one or a combination of a docking line, a cross-section, and a cross-section of the road such that different values of parameters of the geometric design of the road, traffic on the road, traffic rules for traffic flow on the road define different traffic scenarios. By relaxing configuration parameters of a real-world scene and tightening corresponding limiting parameters, a mixed integer non-convex constraint optimization problem for a current real-world traffic scene is converted into a mixed integer convex optimization problem for approximate representation of the real-world traffic scene, so that a carrier is controlled. A mixed integer convex optimization problem for the converted approximate representation of the real-world traffic scene is solved to generate current control commands for controlling one or more actuators of the vehicle.

Description

TECHNICAL FIELD

[0001] The present disclosure relates generally to optimization-based control, and more specifically to methods and apparatus for mixed-integer model predictive control in vehicle decision and motion planning with continuous and discrete operational elements. BACKGROUND

[0002] Conventional autonomous driving vehicles are equipped with control systems that determine how the vehicle should move on a road that satisfies legal driving and traffic rules 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, by computing a desired vehicle path and speed, and by controlling the vehicle to follow that path using available vehicle actuators. Due to the complexity of such conventional operations, some conventional control systems include separate components responsible for path planning and vehicle control. For example, U.S. Patent 9,915,948, incorporated herein by reference, discusses how to integrate vehicle control and path planning to guarantee that a vehicle achieves the desired objectives of a (semi) automated driving system.

[0003] For example, a path plan of an autonomous driving vehicle can execute a motion planning system (MPS) responsible for determining a path and / or a motion trajectory of the vehicle. The MPS can use different path planning methods, see for example U.S. Patent 9,568,915, incorporated herein by reference. To determine a motion trajectory to reach a final goal, the MPS can use knowledge of the current and predicted environment obtained from vehicle sensors or received through a communication channel, and a map of the environment. To be able to adjust the motion trajectory according to changing environments, due to the limited capabilities of the computing and communication platforms in the vehicle, the MPS needs to constantly update the motion trajectory in real-time with limited amount of computation.

[0004] Therefore, due to the need for real-time operation to account for dynamic and rapidly changing environments, the motion trajectory can only be predicted for a short time period, i.e., the planning horizon of the MPS cannot cover the entire driving path of the vehicle, but only a certain sub-segment of the path from the current location to the next intermediate driving goal. In complex dynamic scenarios, such as autonomous driving in urban environments, there can be multiple sequences of intermediate goals that can all lead to successful completion of the trip. On the other hand, due to potential violation of traffic rules and / or collision with other vehicles, some intermediate goals that the vehicle can achieve can not achieve the final driving goal. Therefore, the achievement of some intermediate goals can be practically infeasible for vehicle motion, and if provided to the MPS, the autonomous control of the vehicle will fail.

[0005] Accordingly, autonomous vehicles can execute a decision system (DMS) configured to select intermediate targets to be tracked by the vehicle through its motion trajectory on the path to the final target, see e.g. US20210302974 incorporated herein by reference. However, DMSs are generally difficult to determine intermediate targets achievable by the vehicle with respect to vehicle dynamics, with respect to traffic rules and to avoid collision with any other traffic participants in a dynamically changing environment without being overly conservative and thus reducing the optimality of the automated driving system.

[0006] To this end, there is a need for a simultaneous vehicle decision and motion planning system to discretely decide on a sequence of intermediate targets while simultaneously computing a continuous motion, thereby optimally controlling the motion trajectory that achieves the overall goal of the automated driving system.

[0007] Optimization-based decision, planning and control techniques, such as model predictive control (MPC), allow a model-based design framework in which system dynamics, system requirements and constraints can be directly taken into account. This framework has been extended to hybrid systems, including both continuous and discrete decision variables, which provides a powerful technique to model large-scale problems, e.g. including power systems with mode switching or systems with quantized actuation, problems with logical rules, temporal logic specifications or obstacle avoidance constraints. However, the resulting optimization problems are highly non-convex and thus difficult to solve in practice as they contain variables that only take integer values. When a linear or linear-quadratic objective function is 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 a mixed-integer quadratic program (MIQP). More general convex inequality constraints can be included, such as quadratic inequality constraints, resulting in a mixed-integer quadratic constrained quadratic program (MIQCQP); or second-order cone constraints, resulting in a mixed-integer second-order cone program (MISOCP).

[0008] Mixed-integer model predictive control (MI-MPC) for simultaneous decision and motion planning requires the solution of a mixed-integer program (MIP) at each sampling instant within strict timing constraints. This is a difficult task because the solution of MIPs is generally NP-hard and several approaches for solving such sequences of MIPs have been explored in the literature. These schemes can be divided into heuristics that seek to efficiently find suboptimal solutions to the problem and optimization algorithms that attempt to solve the MIP to optimality. Most mixed-integer optimization algorithms are based on variants of the branch-and-bound (B&B) technique, which solves the MIP to optimality. Variants of the branch-and-bound strategy have been combined with various methods for solving the relaxed convex subproblems, e.g., using dual active set solvers, interior point algorithms, dual projected gradient methods, non-negative least squares solvers, and the multiplier alternating direction method (ADMM). However, as the number of discrete decision variables increases, the combinatorial complexity of the MIP typically leads to an exponential increase in the computation time of the B&B method to solve the MIP, limiting the applicability of MIP-based optimal control designs in practice.

[0009] Examples of heuristic search techniques can be based on rounding and pumping schemes, use approximate optimization algorithms, approximate dynamic programming, or use data-based machine learning techniques, e.g., supervised learning. The use of supervised learning to replicate optimal and feasible MIP solutions from an offline obtained B&B method and to fast online infer these solutions significantly improves the solution time of mixed-integer optimal control problems (MIOCPs). Alternatively, reinforcement learning techniques have been used to learn tree search strategies to speed up the B&B method, but these schemes have limited applicability in practice for real-time embedded systems because they require at least one forward pass of a predictor, e.g., a neural network, at each node of the B&B tree and, more importantly, in the worst case, these schemes can still require the enumeration of the entire B&B tree.

[0010] To this end, there is a need for a computationally efficient scheme that guarantees the finding of a feasible, but possibly suboptimal, solution of the MIP at each sampling instant while meeting real-time requirements with limited computation due to the limited capabilities of the computing and communication platforms in the vehicle. The present invention describes a system and method for simultaneous decision and motion planning in (semi-) autonomous vehicles that uses a combination of a custom branch-and-bound (B&B) method that includes presolving reduction techniques and optimization algorithms to solve the convex relaxations of the MIP and the use of heuristic search techniques that compute feasible, but possibly suboptimal, solutions, e.g., based on (supervised) machine learning to speed up the MIP solution. SUMMARY

[0011] It is an object of some embodiments to provide a simultaneous decision and motion planning system (DM-MPS) in autonomous vehicles configured to select discrete decisions with respect to a sequence of intermediate targets while computing continuous actions to optimally control a motion trajectory that achieves the overall goal of an automated driving system. For example, in some embodiments of the invention, the proposed system aims to follow a route from a current location to a desired destination in a traffic network comprising one or more road segments (each road segment comprising one or more lanes, one or more intersections), and in a dynamically changing environment comprising one or more other traffic participants. Thus, it is an object of some embodiments to cause the DM-MPS to follow the route while satisfying the kinematic constraints of the vehicle, satisfying traffic rules, and avoiding collisions with any other traffic participants in the environment.

[0012] Additionally or alternatively, it is an object of some embodiments to provide a DM-MPS in which the simultaneous computation of discrete and continuous decisions is suitable for real-time execution by an autonomous vehicle computing unit to account for the need to frequently recompute discrete decisions and continuous motion trajectories in accordance with a dynamically changing environment. Furthermore, the DM-MPS needs to always ensure safety, i.e., the simplification of the computation should not compromise the safety of the actual control of the vehicle. However, the design of a DM-MPS that achieves the aforementioned individual objects in practice is challenging.

[0013] Some embodiments of the invention are based on the transformation of the vehicle and its dynamically changing environment from a real-world coordinate system to a road-aligned coordinate system, thereby simplifying the computation at individual sampling instants of the DM-MPS. In some embodiments of the invention, the transformation of the vehicle prediction model results in updated boundary constraints, e.g., updated limits on the steering and / or lateral velocity of the vehicle, to account for the curvature of the road segments along its route. Furthermore, safety and traffic rules can be satisfied by the DM-MPS due to the transformation of the individual traffic participants and their prediction models, as well as the transformation of the individual traffic rules to the road-aligned coordinate system.

[0014] Some embodiments of the invention are based on the recognition that the transformation to the road-aligned coordinate system allows for the 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) or structured mixed-integer quadratic programming (MIQP) problem that can be solved efficiently in real-time. Furthermore, in some embodiments of the invention, after the MIP is solved by the DM-MPS system at individual sampling instants, the computed motion trajectory is again transformed from the road-aligned coordinate system back to the real-world coordinate system, such that the transformed motion trajectory can be executed by the vehicle control system. In some embodiments of the invention, a model predictive controller (MPC) is used to implement the vehicle control system, which aims to follow the continuous reference trajectory computed by the DM-MPS.

[0015] In some embodiments of the invention, the prediction of the future position and / or future speed of one or more traffic participants in the environment needs to be performed by the DM-MPS, in order to compute the optimal and safe discrete decisions and continuous actions of the autonomous driving vehicle over a prediction time window. Some embodiments of the invention are based on the recognition that an increased safety margin needs to be satisfied around the future prediction of the traffic participants, in order to ensure robust safety in the presence of varying environments, perception errors, unknown disturbances, modeling and prediction errors. Furthermore, the DM-MPS needs to continuously update the discrete decisions and motion trajectories of the autonomous driving vehicle in real-time, in order to be robust to these variations, disturbances and errors in the environment.

[0016] In some embodiments of the invention, the DM-MPS system is implemented by solving MILP or MIQP of block structure at each sampling instant, using a combination of a custom Branch-and-Bound (B&B) method and the use of heuristic search techniques, the custom Branch-and-Bound (B&B) method including presolving reduction techniques and optimization algorithms to solve convex relaxations of the MIP, the heuristic search techniques computing feasible but possibly suboptimal solutions, e.g., based on (supervised) machine learning to speed up the MIP solution. Examples of optimization algorithms to solve the convex relaxations are the active-set method, the interior-point method, the (projected) gradient method and the Alternating Direction Method of Multipliers (ADMM).

[0017] Some embodiments of the invention are based on the recognition that presolving reduction techniques can be used to reduce the number of decision variables and / or reduce the number of constraints in the MIP, while preserving feasibility and optimality. In some embodiments of the invention, a presolver is used in each node of the B&B method, reformulating the MIP into a reduced MIP with reduced variables and / or constraints, where the reduced MIP is infeasible or unbounded only when 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 presolving reduction techniques include domain propagation, bound strengthening, dual fixation, implicit variable substitution, coefficient strengthening, probing, 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 Programming (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-solve techniques or on warm starts of MIP solutions at previous sampling times. Some embodiments of the present invention are based on a predictor that, given a set of values ​​of the problem parameters θ, predicts the discrete variables at various time steps i = 0, 1, ..., N in the control horizon of the MIOCP. The optimal value of θ is used to compute a feasible, but potentially suboptimal, solution to the MIP at the current sampling time. Examples of problem parameters θ may include the current state of the autonomous vehicle, traffic regulations, the current states of other traffic participants, the target or final state, actuation constraints, weight values ​​in the objective function, and / or boundary values ​​in the mixed integer inequality constraints of the MI-MPC problem. Some embodiments of the present invention are based on the recognition that the MIP solution from the predictor can be used to reduce the computation of a customized B&B method and ensure real-time feasibility of the simultaneous decision-making and motion planning system.

[0019] Some embodiments are based on the recognition that the complexity of real-world traffic scenarios results in that the optimization of simultaneous decision-making and motion planning of vehicles traveling on roads requires solving mixed-integer non-convex constrained optimization problems. Mixed-integer non-convex constrained optimization problems are difficult to solve in real time, but unfortunately, the geometric design of roads defined by one or a combination of alignments, profiles, and cross-sections of the roads, the traffic on the roads formed by vehicles and other vehicles and pedestrians on the roads, and the traffic rules for traffic flow on the roads used to restrict vehicle actuation result in very complex common traffic scenarios.

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

[0021] Some embodiments are based on the recognition that various real-world traffic scenarios can be represented by a set of parameters. Some parameters, such as the curvature of a road or the shape of a vehicle, can cause non-convexity of the optimization problem. While other parameters, such as the limits on longitudinal velocity and / or acceleration and the limits on lateral velocity and / or acceleration, are irrelevant to the non-convexity, i.e., they do not cause non-convexity of the optimization problem. For example, in some embodiments of the present invention, a convex approximation of a non-linear vehicle kinematic model is used that includes a limit on lateral velocity that depends on longitudinal velocity, thereby avoiding non-convexity of 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 irrelevant to the non-convexity. For example, the curvature parameter of a road can be relaxed to make the road straight by tightening the limits on lateral velocity and / or lateral acceleration of the vehicle. In another example, the parameter of a left or right turn at a traffic intersection can be relaxed to make the road straight by tightening the limits on lateral position of the vehicle and / or by tightening the limits on lateral velocity and / or lateral acceleration of the vehicle. Similarly, the parameter of a turning rate limit of a vehicle can be relaxed to linearize the vehicle model by tightening the limits on lateral velocity and / or lateral acceleration of the vehicle. And another example, the parameter of a physical shape of a vehicle can be relaxed by tightening the limits on position of the vehicle for collision avoidance constraints. In this way, by relaxing parameters of a current real-world traffic scenario that cause non-convexity of a mixed-integer non-convex constrained optimization problem and by tightening at least some parameters of the current real-world traffic scenario that are irrelevant to the non-convexity of the mixed-integer non-convex constrained optimization problem, the mixed-integer non-convex constrained optimization problem can be converted to a mixed-integer convex constrained optimization problem.

[0023] In some embodiments of the present invention, the proposed system and method for vehicle decision and motion planning use, at each control time step, a relaxation of configuration parameters and a tightening of one or more limit parameters from one or more parameters of a real-world traffic scenario to a mixed-integer convex programming (MICP) approximate representation of the real-world traffic scenario. In some embodiments of the present invention, a real-world traffic scenario of a vehicle motion planning problem of driving on a curved road segment with one or more other vehicles is converted to an approximate representation of a vehicle motion planning problem of driving on a straight road segment with one or more other vehicles, the approximate representation having tightened limit parameter values (e.g., tightened boundary values of one or more constraint functions in the MICP-based traffic scenario approximate representation), the approximate representation containing a (piecewise) linear dynamics model of the (semi-) autonomous driving vehicle and a simplified representation of the vehicle environment in the traffic scenario. According to some embodiments of the present invention, the MICP solution includes an optimal motion trajectory of the vehicle that is additionally converted back from the approximate representation of the traffic scenario to the real-world representation, thereby controlling the motion of the (semi-) autonomous vehicle in the real-world traffic scenario.

[0024] Some embodiments of the present application are based on the recognition that the MICP problem is a convexly constrained optimization problem, which is computationally inexpensive to solve for fixed value sets of individual integer variables, and thus can be efficiently used to compute fixed value sets of individual integer variables in the optimal MICP solution using computationally efficient branch-and-bound methods and / or machine learning based techniques.

[0025] Accordingly, one embodiment discloses a controller for controlling a vehicle travelling on a road having a geometric design defined by one or a combination of a tangent, a break, and a cross section of the road, wherein different values of parameters of the geometric design of the road, traffic on the road, traffic rules for traffic flow on the road define different traffic scenarios, the controller comprising: at least one processor; and a memory having instructions stored thereon that, when executed by the at least one processor, cause the controller to: collect parameters for controlling the vehicle for a current real-world traffic scenario, the parameters including configuration parameters that cause non-convexity of a mixed-integer non-convex constrained optimization problem for simultaneous decision and motion planning for the vehicle and limit parameters that are independent of the non-convexity of the mixed-integer non-convex constrained optimization problem; convert the mixed-integer non-convex constrained optimization problem for the current real-world traffic scenario to a mixed-integer convex optimization problem for an approximate representation of the real-world traffic scenario by relaxing the configuration parameters and tightening corresponding limit parameters; solve the converted mixed-integer convex optimization problem for the approximate representation of the real-world traffic scenario to produce current control commands for controlling one or more actuators of the vehicle; and control the one or more actuators of the vehicle in accordance with the control commands.

[0026] Another embodiment discloses a method for controlling a vehicle traveling on a road having a geometric design defined by one or a combination of a tangent line, a profile, and a cross-section of the road, wherein different values of parameters of the geometric design of the road, traffic on the road, traffic rules for traffic flow on the road define different traffic scenarios, wherein the method uses a processor coupled to a memory having instructions stored thereon that, when executed by the processor, perform steps of the method, the steps comprising: collecting parameters for controlling the vehicle for a current real-world traffic scenario, the parameters including configuration parameters that cause non-convexity of a mixed-integer non-convex constrained optimization problem for simultaneous decision 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; converting the mixed-integer non-convex constrained optimization problem for the current real-world traffic scenario into a mixed-integer convex optimization problem for an approximate representation of the real-world traffic scenario by relaxing the configuration parameters and tightening corresponding limiting parameters; solving the converted mixed-integer convex optimization problem for the approximate representation of the real-world traffic scenario to produce 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.

[0027] Yet another embodiment discloses a non-transitory computer-readable storage medium having implemented thereon a program executable by a processor for performing a method for controlling a vehicle traveling on a road having a geometric design defined by one or a combination of a tangent line, a profile, and a cross-section of the road, wherein different values of parameters of the geometric design of the road, traffic on the road, traffic rules for traffic flow on the road define different traffic scenarios, the method comprising: collecting parameters for controlling the vehicle for a current real-world traffic scenario, the parameters including configuration parameters that cause non-convexity of a mixed-integer non-convex constrained optimization problem for simultaneous decision 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; converting the mixed-integer non-convex constrained optimization problem for the current real-world traffic scenario into a mixed-integer convex optimization problem for an approximate representation of the real-world traffic scenario by relaxing the configuration parameters and tightening corresponding limiting parameters; solving the converted mixed-integer convex optimization problem for the approximate representation of the real-world traffic scenario to produce 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 DESCRIPTION OF DRAWINGS

[0028] [ FIG. 1A ]FIG. 1A A block diagram of a mixed-integer predictive controller and feedback system is shown, in accordance with some embodiments.

[0029] [ FIG. 1B ] FIG. 1B A block diagram of an implementation of a mixed-integer predictive controller and feedback system is shown, in accordance with some embodiments of the present application.

[0030] [ FIG. 1C ] FIG. 1C A block diagram of an implementation of a mixed-integer predictive controller and feedback system is shown, in accordance with some embodiments of the present application.

[0031] [ FIG. 1D ] FIG. 1D A block diagram of a multi-layer control architecture and feedback system including a vehicle decision and motion planning system is shown, in accordance with some embodiments of the present application.

[0032] [ FIG. 2A ] FIG. 2A A schematic of a vehicle including a predictive controller employing principles of some embodiments is shown.

[0033] [ FIG. 2B ] FIG. 2B A schematic of an interaction between a predictive controller and other controllers of a vehicle is shown, in accordance with some embodiments.

[0034] [ FIG. 2C ] FIG. 2C A schematic of a path and / or motion planning method for a controlled vehicle employing principles of some embodiments is shown.

[0035] [ FIG. 2D ] FIG. 2D An exemplary traffic scenario based on a single-vehicle or multi-vehicle decision module of some embodiments is shown.

[0036] [ FIG. 3A ] FIG. 3A A block diagram of a transformation from a real-world representation to an approximate representation of a constraint optimization problem in a vehicle decision and motion planning system for a controlled vehicle is shown, in accordance with some embodiments of the present application.

[0037] [ FIG. 3B ] FIG. 3B A block diagram of a transformation and solution strategy, including a first transformation step, a solution step of the MICP problem, followed by a second inverse transformation step to compute an optimal motion trajectory and control action sequence in a real-world traffic scenario is shown.

[0038] [ FIG. 3C ] FIG. 3CA block diagram showing the conversion and solution strategy according to some embodiments, including relaxing one or more configuration parameters that lead to non-convexity, and including tightening one or more limit parameters that are not related to the non-convexity of the constrained optimization problem for the vehicle decision and motion planning.

[0039] [ FIG. 4A ] FIG. 4A A block diagram showing the conversion of a vehicle decision and motion planning problem in an exemplary traffic scenario with one or more other vehicles on a curved road segment from a real-world representation to an approximate representation in a constrained mixed-integer convex optimization problem with tightened limit parameter values.

[0040] [ FIG. 4B ] FIG. 4B A block diagram showing the inverse conversion of an optimal solution of a mixed-integer convex optimization problem with tightened limit parameter values to an approximate solution for a mixed-integer non-convex optimization problem for the real-world representation according to some embodiments.

[0041] [ FIG. 5 ] FIG. 5 A block diagram showing the conversion steps and inverse conversion steps for a vehicle decision and motion planning problem in an exemplary traffic scenario with a controlled vehicle with one or more other vehicles near a traffic intersection according to some embodiments of the invention.

[0042] [ FIG. 6A ] FIG. 6A A block diagram showing the system and method of a mixed-integer model predictive controller (MI-MPC) for computing control signals given the current state and control commands of the system in the proposed vehicle decision and motion planning system according to some embodiments.

[0043] [ FIG. 6B ] FIG. 6B A block diagram showing the MI-MPC method that solves an optimal control structured mixed-integer linear quadratic optimization problem to compute control signals at individual control time steps given the current state and commands of the vehicle to compute a motion plan including a sequence of future discrete decisions and continuous actions for the controlled vehicle.

[0044] [ FIG. 6C ] FIG. 6C A block diagram showing the MIMPC method according to some embodiments of the invention that finds a feasible but possibly suboptimal solution vector for an optimal control structured MICP given the current problem parameter values and commands to compute control signals at individual time steps to compute a motion plan including a sequence of future discrete decisions and continuous actions for the controlled vehicle.

[0045] [ FIG. 7A ] FIG. 7AOne or more obstacle avoidance constraints using mixed integer inequality constraints are shown that force the controlled vehicle to be within one of a plurality of disjoint regions outside a safety region around an obstacle in a traffic network, in accordance with some embodiments of the application.

[0046] [ FIG. 7B ] FIG. 7B One or more obstacle avoidance constraints for a controlled vehicle in an approximate representation of an exemplary real-world traffic scenario with one or more other vehicles are shown used in the proposed vehicle decision and motion planning system.

[0047] [ FIG. 7C ] FIG. 7C One or more obstacle avoidance constraints used in the proposed vehicle decision and motion planning system are shown in an approximate representation of an exemplary real-world traffic scenario with one or more other vehicles and one or more traffic intersections along a particular route of the controlled vehicle in a traffic network, in accordance with some embodiments of the application.

[0048] [ FIG. 7D ] FIG. 7D One or more spatially dependent region constraints for a controlled vehicle in an approximate representation of an exemplary real-world traffic scenario with one or more other vehicles are shown used in the proposed vehicle decision and motion planning system, in accordance with some embodiments of the application.

[0049] [ FIG. 8A ] FIG. 8A A schematic diagram of an example of a binary control variable search tree representing a nested tree of search regions for integer feasible control solutions is shown, in accordance with some embodiments of the application.

[0050] [ FIG. 8B ] FIG. 8B A block diagram of a branch-and-bound mixed integer optimization algorithm for searching for integer feasible optimal control solutions based on a nested tree of search regions and corresponding lower / upper bound values is shown, in accordance with some embodiments of the application.

[0051] [ FIG. 9A ] FIG. 9A An optimal control structured MICP problem can be very efficiently converted to an optimal control structured convex programming (CP) problem and solved after fixing all discrete variables to a fixed set of values, in accordance with some embodiments of the application.

[0052] [ FIG. 9B ] FIG. 9B A block diagram of a prediction-based MICP solving method followed by correction of the fixed discrete value set and solution of an optimal control structured CP is shown, in accordance with some embodiments.

[0053] [FIG. 9C ] FIG. 9C A block diagram of a MICP solution approach based on multiple predictions followed by correction of fixed discrete value sets and solution of optimal control structured CP is shown, in accordance with some embodiments.

[0054] [ FIG. 9D ] FIG. 9D A block diagram of a MICP solution approach based on predictions for a first subset of fixed discrete variables followed by correction based on pre-solution reduction for a remaining subset of fixed discrete variables and solution of optimal control structured CP is shown, in accordance with some embodiments.

[0055] [ FIG. 10A ] FIG. 10A A block diagram of offline data generation and supervised learning process for training machine learning based predictors used in online variable fixing and optimal control solution process to solve MICP in vehicle decision and motion planning systems is shown.

[0056] [ FIG. 10B ] FIG. 10B A flow diagram of offline data generation process based on MICP solutions for sampled problem parameter value sets and supervised learning process for training machine learning based predictors, in accordance with some embodiments of the present invention, is shown.

[0057] [ FIG. 10C ] FIG. 10C A flow diagram of online variable fixing process based on predictions and iterative pre-solution based correction steps and optimal control structured CP solution process for computing MICP solutions in vehicle decision and motion planning systems, in accordance with some embodiments, is shown. DETAILED DESCRIPTION

[0058] Some embodiments of the present disclosure provide a system and method for controlling operation of a vehicle using a predictive controller. An example of a predictive controller is a model predictive controller (MPC) that 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, motion planning, and / or trajectory generation with continuous and discrete operational elements.

[0059] Some embodiments are based on the recognition that the complexity of real-world traffic scenarios makes simultaneous decision and motion planning for a vehicle driving on a road require the solution of a mixed-integer non-convex constrained optimization problem. Mixed-integer non-convex constrained optimization problems are difficult to solve in real-time, but unfortunately the geometric design of a road defined by one or a combination of its 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 for limiting the vehicle actuation for the traffic flow on the road form a very complex general traffic scenario.

[0060] 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 combined with mixed-integer equality and / or inequality constraints imposing traffic rules such as collision avoidance constraints, lane change constraints, and / or traffic intersection stop constraints. Examples of real-world traffic scenarios for vehicle decision and motion planning can include a (semi-) autonomous vehicle and one or more other vehicles driving in a complex environment 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 stop zones, one or more traffic lights, and / or one or more merging points.

[0061] Some embodiments are based on the recognition that individual real-world traffic scenarios can be represented by a parameter set. Some parameters, such as the curvature of a road or the shape of a vehicle, can lead to non-convexity of the optimization problem. While other parameters, for example, limits on longitudinal velocity and / or acceleration and limits on lateral velocity and / or acceleration, are irrelevant for non-convexity, i.e., they do not lead to non-convexity of the optimization problem. For example, in some embodiments of the invention, a convex approximation of a nonlinear vehicle kinematic model is used that includes a limit on lateral velocity that depends on longitudinal velocity, thereby avoiding non-convexity of the optimization problem.

[0062] Some embodiments are based on the recognition that parameters that cause non- convexity can be relaxed at the expense of parameters that are not associated with non- convexity. For example, a curvature parameter of a road can be relaxed to make the road straight by tightening a limit on a lateral velocity and / or a lateral acceleration of a vehicle. In another example, a parameter of a left turn or a right turn at a traffic intersection can be relaxed to make the road straight by tightening a limit on a lateral position of a vehicle and / or by tightening a limit on a lateral velocity and / or a lateral acceleration of the vehicle. Similarly, a parameter of a turning rate limit of a vehicle can be relaxed to linearize a vehicle model by tightening a limit on a lateral velocity and / or a lateral acceleration of the vehicle. And another example, for a collision avoidance constraint, a parameter of a physical shape of a vehicle can be relaxed by tightening a limit on a position of the vehicle. In this way, by relaxing parameters of a current real-world traffic scenario that cause non-convexity of a mixed-integer non-convex constrained optimization problem and by tightening at least some parameters of the current real-world traffic scenario that are not associated with non-convexity of the mixed-integer non-convex constrained optimization problem, the mixed-integer non-convex constrained optimization problem can be converted to a mixed-integer convex constrained optimization problem.

[0063] In some embodiments of the invention, the proposed system and method for vehicle decision and motion planning use a conversion of configuration parameters from one or more parameters of a real-world traffic scenario to a mixed-integer convex programming (MICP) approximated representation of the real-world traffic scenario at each control time step with relaxation of the configuration parameters and tightening of one or more limit parameters. In some embodiments of the invention, a real-world traffic scenario of a vehicle motion planning problem of driving on a curved road segment with one or more other vehicles is converted to an approximated representation of the vehicle motion planning problem of driving on a straight road segment with one or more other vehicles with tightened limit parameter values (e.g., tightened boundary values of one or more constraint functions in the MICP-based traffic scenario approximated representation) that contain a (piecewise) linear dynamics model of the (semi-)autonomous driving vehicle and a simplified representation of the vehicle environment in the traffic scenario. According to some embodiments of the invention, the MICP solution includes an optimal motion trajectory of the vehicle that is additionally converted back from the approximated representation of the traffic scenario to the real-world representation to control the motion of the (semi-)autonomous vehicle in the real-world traffic scenario.

[0064] Some embodiments of the invention are based on the recognition that a MICP problem is a convex constrained optimization problem that is computationally inexpensive to solve for a fixed value set of each integer variable and, thus, can be efficiently computed for a fixed value set of each integer variable in the optimal MICP solution using computationally efficient branch-and-bound methods and / or machine learning-based techniques.

[0065] FIG. 1AA block diagram of a predictive controller 110 and a feedback system for a controlled (semi) autonomous vehicle 120 is shown, in accordance with some embodiments. FIG. 1A An example feedback system is shown, in accordance with some embodiments of the present application, which includes a vehicle 120 connected to a 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. Examples of feedback signals from the traffic environment 145 include a state of one or more neighboring vehicles around the controlled vehicle 120, where the state includes position, shape, and / or velocity. 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 required to stop in a desired crossing direction through a traffic intersection. Other examples of feedback signals from the environment 145 include one or more traffic signs, for example, including a stop sign, a speed limit sign, and / or a priority traffic sign. In some embodiments of the present application, the sensors and communication devices 140 provide information about a geometric design of a road defined by one or a combination of alignments, profiles, and cross-sections of the road, traffic on the road formed by the vehicle and other vehicles and pedestrians on the road, and traffic rules for limiting a flow of traffic on the road for actuation of the vehicle 120.

[0066] In some embodiments of the present application, 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 can be a set of equations representing a state and outputs 103 of the vehicle 120 as a function of current and previous inputs 111 and previous outputs 103 over time. The system model 102 can include constraints 104 representing physical and operational limitations of the system 120. During operation, the controller 110 receives a command 101 indicative of a desired behavior of the vehicle 120. For example, the command can be a motion command and / or a route in a traffic network to travel from a current location to a desired destination. In response to receiving the command 101, the controller 110 generates a control signal 111 to be used as an input to the controlled vehicle 120. In response to the input, the system updates the outputs 103 of the vehicle 120. Based on a measurement of the outputs 103 of the vehicle 120, the estimator 130 updates an 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 accepts a feedback signal 121 of the vehicle 120 via the estimator 130, where the feedback signal 121 includes a measurement of a state of the vehicle 120, and the predictive controller accepts one or more feedback signals 122 of a traffic environment 145 of the vehicle 120 using the sensors and communication devices 140.

[0067] As described herein, the controlled system 120 can be any vehicle, including a two- wheeled vehicle (such as a motorcycle), a four-wheeled vehicle (such as a passenger car), or a vehicle with more than four wheels (such as a truck, etc.). The vehicle 120 is controlled by certain manipulated input signals (e.g., control signals 111 (input)) that can be associated with physical quantities such as voltage, pressure, force, torque, and returns some controlled output signals 103 (output) that can be associated with physical quantities such as wheel speed, angular velocity, acceleration, velocity, indicating a transition of the state of the controlled vehicle 120 from a previous state to a current state. The output values are partially dependent on previous output values of the system and partially dependent on previous and current input values. The dependence on previous inputs and previous outputs is encoded in the state of the controlled vehicle 120.

[0068] The system model 102 can include a set of mathematical equations that describe how the system outputs change over time as a function of current and previous inputs and previous outputs. The state of the vehicle 120 is any collection of information that typically changes over time, such as a suitable subset of current and previous inputs and outputs, which together with the model of the system and future inputs can uniquely define the future motion of the system.

[0069] The controlled vehicle 120 can be subject to physical limitations and specification constraints 104 that limit the range of outputs, inputs, and possibly states that the system 120 is allowed to operate. Examples of constraints 104 include safety distance constraints, speed limits, acceleration and deceleration constraints, steering rate limits, turning radius constraints, lane change constraints, and / or lane change timing constraints.

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

[0071] In some embodiments of the invention, heuristic search techniques are used to compute the control signal 111, e.g., using a round-and-pump scheme, using an approximate optimization algorithm, approximate dynamic programming, or using data-based machine learning techniques to find a feasible, but possibly suboptimal, approximation of the MIP solution. For example, supervised learning can be used to train a deep neural network architecture to implement a predictive model that maps parameter set values to MIP solutions and / or control signals 111. In some embodiments of the invention, the predictive model can be deterministic, e.g., including multilayer perceptrons, deep sets, convolutional neural networks (CNNs), recurrent neural networks (RNNs), kernel regressions, support vector machines, and other machine learning algorithms or any combination of such deterministic predictive models. Alternatively, in some embodiments of the invention, the predictive model can be stochastic, e.g., including Bayesian neural networks, neural processes, Gaussian processes, Kriging interpolation, and other machine learning algorithms or any combination of such deterministic and / or stochastic predictive models.

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

[0073] FIG. 1B A block diagram of the predictive controller 110 and feedback system 120 is shown, according to some embodiments. The predictive controller 110 actuates the vehicle 120 so that the estimated state 121 and output 103 of the vehicle 120 follow the commands 101. The controller 110 includes a computer, e.g., in the form of a single central processing unit (CPU) or multiple CPU processors 151 connected to a memory 152 for storing the system model 102 and constraints 104 on the operation of the controlled vehicle 120. The CPU processors 151 can include single-core processors, multi-core processors, computing clusters, or any number of other configurations. The memory 152 can include random access memory (RAM), read-only memory (ROM), flash memory, or any other suitable memory system.

[0074] FIG. 1C A block diagram of the predictive controller 110 and feedback system 120 is shown, according to some embodiments, in which the memory 152 includes one or more sections. In some embodiments of the invention, the memory 152 includes a first section 152a for storing information about the vehicle and 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 motion models of traffic.

[0075] For example, the first section 152a of the memory 152 can store parameters for the behavior of the vehicle, such as maximum acceleration, steering and steering rate, as well as a first motion model of the vehicle and a second motion model of the vehicle. In various embodiments, the number and complexity of the equations describing the second motion model of the vehicle is higher than the number and complexity of the equations describing the first motion model of the vehicle. Further, for example, the fourth section 152d of the memory 152 can store a first motion model of the traffic and a second motion model of the traffic.

[0076] Still referring to FIG. 1C In various embodiments, the number and complexity of the equations describing the second motion model of the traffic is higher than the number and complexity of the equations describing the first motion model of the traffic. These embodiments are based on the recognition that for checking what intermediate goals the vehicle can achieve in the near future, and for generating trajectories and for controlling the vehicle according to such trajectories, different motion models are necessary to use. For example, in order to check whether the vehicle can achieve a series of goals, long-term future times need to be considered. Calculating the motion of the vehicle over extended future times with a high-order physical model is computationally difficult. In contrast, when intermediate goals are known, controlling the vehicle according to a desired trajectory can only consider shorter future times. To this end, in some embodiments, the controller 110 uses a first (i.e. low-order) motion model to determine the next goal, while planning and control use at least a second (i.e. high-order) motion model.

[0077] According to some embodiments, the second section 152b of the memory 152 can be embedded with a program executable by the processor 151 for performing a method for controlling the vehicle 120.

[0078] Still referring to FIG. 1C The third section 152c of the memory 152 comprises map information, such as addresses and road networks, and it can also comprise additional information, such as intersections, parking and traffic light locations, number and location of lanes, speed limits, traffic rules, curvature of road segments, etc. The map information can already be stored in the third section of the memory 152c when the vehicle starts driving, or alternatively, this information is made available to the control unit by the sensors and communication means 140.

[0079] The processor 151 can be any computing device capable of performing calculations, and can comprise one or more physical devices of the same or different type. The processor 151 can comprise a plurality of computing devices, e.g. microprocessors. Similarly, the memory 152 can be any logical memory and / or non-transitory computer readable storage medium capable of storing information, and can comprise one or more physical information storage devices of the same or different type. The calculations performed by the processor 151 are by program commands stored in the second section of the memory 152b, and using the vehicle information stored in the first section of the memory 152a, the information about the map stored in the third section of the memory 152c, the information about the vehicle 152a obtained from the sensor output 103, the traffic information 152d of the environment 145 obtained from the sensors and communications 140. The calculations of the processor 151 result in control inputs 111 that change the motion of the vehicle.

[0080] FIG. 1D A block diagram of a feedback control system according to some embodiments of the present application is shown, the feedback control system comprising a multi-layer architecture comprising one or more components coupled to each other to compute control signals 111 to control the motion of the vehicle 120. The program executed by the processor 151 enables autonomous driving (AD) of the vehicle 120, here AD is intended to also include semi-autonomous driving. During this operation, the program executed by the processor 151 is intended to achieve a specific overall goal of the driving, e.g. reaching a specific location. The overall goal is achieved by appropriately affecting the motion of the vehicle 120. The software program executed by the processor 151 can be logically divided into a plurality of modules. For example, in one embodiment, the program executed by the processor comprises at least two modules arranged in order as layers, such that the output of one layer is the input of the next layer. As used herein, this layering designates layers or logical modules of the control unit 160, and allows the control to be separated into different stages that require different information.

[0081] FIG. 1D A schematic diagram of the layers of the control unit 160 according to one embodiment of the present disclosure is shown. In this embodiment, the control unit 160 comprises four layers of control. Information about the state of the vehicle 121, as well as feedback signals 122 from the sensors and communications 140 of the traffic environment 145 are provided to the different individual layers. The path planning module 161 uses the information stored in the first section of the memory 152a, the second section of the memory 152b, and the third section of the memory 152c, as well as the information obtained from the sensor output 103 and the traffic information 152d of the environment 145 obtained from the sensors and communications 140, to compute the control signals 111 to control the motion of the vehicle 120. FIG. 1CThe route module determines a sequence of roads in the road network that the vehicle traverses from its current location to reach its desired destination, using map information in a third segment of memory 152c and the current location of the vehicle obtained from sensor outputs 103. The desired destination can be provided, for example, by a human user. The route module can be implemented by a car navigation system. According to some embodiments of the present application, the decision and motion planning module 162 aims to make discrete decisions about a sequence of intermediate targets while computing continuous actions, so as to optimally control a motion trajectory that achieves the overall goal of the autonomous driving system.

[0082] In some embodiments of the present application, the decision and motion planning module 162 uses information from the current state of the vehicle 121, feedback signals 122 from the traffic environment 145, and from at least a portion of the sequence of roads in the road network that the vehicle traverses from its current location to reach its desired destination, and the module 162 determines a sequence of one or more intermediate targets and a sequence of one or more continuous actions, so as to produce a feasible and / or optimal motion trajectory that is provided to the vehicle control module 163. The vehicle control module 163 determines commands to the vehicle actuators, such as steering, acceleration, deceleration, that modify the behavior of the vehicle so that it achieves an actual trajectory that is as close as possible to the motion trajectory provided by the decision and motion planning module 162. The actuator control sub-module 164 then receives the commands to the vehicle actuators that modify the control signals to the actuators, e.g., motor voltages, throttle opening, brake pad pressure, to achieve the desired vehicle commands.

[0083] In some embodiments of the present application, the decision and motion planning module 162 is implemented using a MI-MPC controller that solves MIPs at each sampling time period to compute a sequence of discrete decisions and continuous control actions over a prediction time window. In some embodiments of the present application, the vehicle control module 163 is implemented using a linear, linear time-varying, or nonlinear MPC controller that solves convex quadratic programming (QP) or non-convex nonlinear programming (NLP) problems at each sampling time period to compute a sequence of vehicle actuation commands so that the vehicle achieves an actual trajectory that is as close as possible to the motion trajectory. Some embodiments of the present application are based on the recognition that NLPs can be solved using sequential convex programming (SCP) or sequential quadratic programming (SQP) techniques, and that each convex QP sub-problem can be solved using a convex optimization algorithm such as, for example, an active-set solver, an interior-point algorithm, a projected gradient method, a non-negative least squares solver, or a multiplier alternating direction method of multipliers (ADMM).

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

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

[0086] FIG. 2A A schematic diagram of a vehicle 201 including a predictive controller 202 employing principles of some embodiments of the invention is shown. As used herein, the vehicle 201 can be any type of wheeled vehicle, such as a passenger car, a bus, or a rover. Furthermore, the vehicle 201 can be an autonomous or semi-autonomous vehicle. For example, some embodiments control the motion of the vehicle 201. Examples of motion include lateral motion of the vehicle 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 can be controlled by a driver of the vehicle 201.

[0087] The vehicle can also include an engine 206, which can be controlled by the controller 202 or other components of the vehicle 201. The vehicle can also include one or more sensors 204 to sense the surrounding environment in the traffic scene, e.g., including other vehicles, traffic signs, traffic light signals, lane boundaries, and / or road curvatures. Examples of sensors 204 include range finders, radars, lidars, and cameras. The vehicle 201 can also include one or more sensors 205 to sense its current amount of motion and internal state. Examples of sensors 205 include global positioning systems (GPS), accelerometers, inertial measurement units, gyroscopes, axle rotation sensors, torque sensors, deflection sensors, pressure sensors, and flow sensors. The sensors provide information to the controller 202. The vehicle can be equipped with a transceiver 206, which enables the communication capabilities of the controller 202 over wired or wireless communication channels.

[0088] FIG. 2B A schematic diagram showing the interaction between the controller 202 (i.e., the mixed-integer predictive controller) of the vehicle 201 and other controllers 220 according to some embodiments is shown. In some embodiments of the present application, the other controllers 220 include the vehicle control module 163 and / or the actuator control module 164, as shown, for example. In some embodiments of the present application, the controllers 220 of the vehicle 201 are the steering 225 and brake / throttle controllers 230 that control the rotation and acceleration of the vehicle 201, respectively. In this case, the mixed-integer predictive controller 202 outputs control inputs to the controllers 225 and 230 to control the state of the vehicle 201. FIG. 1D

[0089] In some embodiments of the present application, the controllers 220 include high-level controllers, such as a lane-keeping assist controller 235 and / or a reference trajectory tracking controller 240, that further process the control inputs of the mixed-integer predictive controller 202. In some embodiments of the present application, the controllers 220 use the outputs of the mixed-integer predictive controller 202 to control at least one actuator of the vehicle 201, such as the steering wheel and / or the brakes of the vehicle 201, thereby controlling the motion of the vehicle 201. Further, the mixed-integer predictive controller 202 determines inputs to the vehicle 201 based on the mixed-integer control solution, where the inputs to the vehicle 201 include one or a combination of the acceleration of the vehicle 201, the engine torque of the vehicle 201, the brake torque, and the steering angle of the vehicle 201, as well as discrete optimization variables to model one or a combination of discrete control decisions, switches in system dynamics, gear shifts, lane change commands, and obstacle avoidance constraints.

[0090] ​In some embodiments of the present application, the reference trajectory tracking controller 240 is implemented using a linear, linear time-varying, or nonlinear MPC controller that solves a convex quadratic programming (QP) or non-convex nonlinear programming (NLP) problem at each sampling period to compute a sequence of vehicle actuation commands such that the vehicle achieves an actual trajectory that is as close as possible to the motion trajectory computed by the mixed-integer predictive controller 202. Some embodiments of the present application are based on the recognition that NLPs can be solved using sequential convex programming (SCP) or sequential quadratic programming (SQP) techniques, and that each convex QP subproblem can be solved using a convex optimization algorithm such as, for example, an active-set solver, an interior-point algorithm, a projected gradient method, a non-negative least squares solver, or a multiplier alternating direction method (ADMM).

[0091] FIG. 2C A schematic of a path and / or motion planning method for a controlled vehicle employing some embodiments is shown. Further, FIG. 2C A schematic of an autonomous or semi-autonomous controlled vehicle 250 is shown for which a dynamically feasible and generally optimal trajectory 255 can be computed by using embodiments of the present disclosure. The generated trajectory is intended to keep the vehicle within certain road boundaries 252 and to avoid other controlled and / or uncontrolled vehicles, i.e., these vehicles are obstacles 251 to the particular controlled vehicle 250. In some embodiments, each obstacle 251 can be represented by one or more inequality constraints in the temporal or spatial equations of the constrained mixed-integer programming problem, including one or more additional discrete variables for each obstacle. For example, based on embodiments configured to implement a mixed-integer model predictive controller, the autonomous or semi-autonomous controlled vehicle 250 can make discrete decisions in real-time, such as, for example, deciding whether to perform a lane change, lane keeping, or stopping, whether to pass another vehicle on the left or right, or alternatively, to stay behind another vehicle in the current lane of the road 252, while additionally making continuous decisions in real-time, such as, for example, speed, acceleration, or steering inputs to control the motion of the vehicle 250.

[0092] FIG. 2D An exemplary traffic scenario based on single-vehicle or multi-vehicle decision modules is shown. FIG. 2DA scenario is depicted with one or more controlled ego vehicles 271, traffic composed of other vehicles like the one shown as 272, a lane marked as L6, e.g., 273, a stop line marked as SI, e.g., 274, and an intersection marked as I3, e.g., 275. For a vehicle in location 261 with a final destination 262, the path planning module 161 provides a sequence of roads indicated by arrows 263 and a sequence of turns indicated by arrows 264. It should be noted, however, that the road sequence 263 and the turn sequence 264 do not by themselves specify a trajectory or path for the vehicle. There are many discrete decisions to be made, e.g., which lane the vehicle will travel in, whether the vehicle should change lanes or stay on the current lane, whether the vehicle should start to decelerate to stop at the stop line, whether the vehicle is allowed to cross the intersection, etc. Furthermore, there are many continuous decisions to be made, e.g., a timed sequence of positions and orientations that the vehicle should reach in its travel from its initial point to its destination. These decisions are highly dependent on the current traffic at the time the vehicle reaches the respective locations, which is typically unknown to the route module due to the uncertainty of traffic movements and the uncertainty of the time at which the vehicle will reach the location. In some embodiments of the present disclosure, the motion plan for one or more controlled ego vehicles 271 can be computed by solving one or more connected mixed-integer programming problems, possibly with communication to allow coordination between vehicles (V2V) and / or between the intelligent infrastructure system and the vehicles (V2X).

[0093] Some embodiments of the present disclosure are based on the recognition that obtaining accurate predictions of the behavior of one or more other vehicles (e.g., human-driven vehicles (HDVs)) in the traffic environment near a controlled vehicle 120 can prove challenging, depending on the sensing infrastructure of the traffic network. Because of this recognition, in some embodiments of the present disclosure, the vehicle decision and motion planning system (DM-MPS) is implemented as a receding horizon manner as a mixed-integer predictive controller 110, where a constrained MIP is solved at each sampling time step based on the latest information from sensors and communication devices 140. For example, in some embodiments of the present disclosure, the sampling time period is 0.25 to 0.5 seconds long for the decision and motion planning module 162, and 0.025 to 0.05 seconds long for the vehicle control module 163. Any discrepancies in the predictions of the behavior of one or more other vehicles can be adjusted by the inherent feedback mechanism of the receding time policy for the vehicle control architecture, which includes at least the decision and motion planning module 162 and the vehicle control module 163.

[0094] FIG. 3AA block diagram illustrating the conversion from a real-world representation to an approximate representation of a constrained optimization problem in a vehicle decision and motion planning system for a controlled vehicle is shown in accordance with some embodiments of the present application. Some embodiments are based on the recognition that the complexity of a real-world traffic scenario 305 in a road map environment 310 and the need for a controlled vehicle 120 to satisfy complex traffic rules 311 given the current state of the controlled vehicle 301 makes the constrained optimization of simultaneous decision making and motion planning of a vehicle driving on a road a mixed integer non-convex constrained optimization problem 315 that needs to be formulated and solved. The mixed integer non-convex constrained optimization problem 315 is difficult to solve in real-time, but unfortunately, the road's junctions, sections, and cross-sections, one or a combination of which, traffic on the road formed by the vehicle as well as other vehicles and pedestrians in the real-world traffic scenario 305, and complex traffic rules 311 that limit the vehicle's actuation make a highly complex vehicle decision and motion planning problem.

[0095] In some embodiments of the present application, the conversion and solution of a mixed integer convex programming (MICP) problem is used to compute an approximation of the optimal motion trajectory and optimal sequence of actions of the controlled vehicle 320, such that an approximation to the mixed integer non-convex constrained optimization problem 315 can be computed in a computationally efficient manner. In accordance with some embodiments of the present application, the MICP solution 320 includes a reference motion trajectory that is used by the vehicle control system 325, such that the controlled vehicle 120 achieves an actual trajectory that is as close as possible to the motion trajectory provided by the decision and motion planning module, as described in FIG. 1D

[0096] In some embodiments of the present application, examples of the mixed integer non-convex constrained optimization problem include non-linear vehicle dynamics and / or non-linear road boundary constraints combined with mixed integer equality and / or inequality constraints to impose traffic rules, such as collision avoidance constraints, lane change constraints, and / or traffic intersection stop constraints. Examples of real-world traffic scenarios for vehicle decision and motion planning can include a (semi-) autonomous vehicle and one or more other vehicles driving in a complex environment with one or more connected road segments having one or more lanes, one or more speed zones, one or more traffic intersections, one or more stop zones, one or more traffic lights, and / or one or more merging points.

[0097] ​In some embodiments of the application, the proposed vehicle decision and motion planning system is implemented using a predetermined traffic scenario that results in a mixed integer convex optimization problem, in accordance with some embodiments of the application, and the conversion and solving process 320 converts one or more parameters of the current real-world traffic scenario to one or more parameters of the predetermined traffic scenario, and the resulting MICP forms an approximate representation of a constrained optimization problem in the vehicle decision and motion planning system for the controlled vehicle in the real-world traffic scenario. For example, in some embodiments of the application, the predetermined traffic scenario includes the controlled vehicle on a straight road segment with one or more lanes using a road-aligned coordinate system, and includes predicted motion trajectories of one or more other vehicles and / or traffic participants, one or more obstacle avoidance constraints for the controlled vehicle to remain outside of a safety region around the predicted motion trajectories of the one or more other vehicles and / or traffic participants, one or more conflict region constraints for each traffic intersection and / or merging point, and one or more spatially dependent region constraints along the future planned route of the controlled vehicle in the traffic network.

[0098] FIG. 3B A block diagram of the conversion and solving strategy for computing an approximation of the optimal motion trajectory and optimal sequence of actions of the controlled vehicle 320 is shown, including a first conversion step 330 for computing a mixed integer convex approximation of the vehicle decision and motion planning problem, a solving step 340 of the MICP problem for computing an approximate representation of the optimal motion trajectory and control action sequence, followed by a second inverse conversion step 345 from the approximate representation of the optimal motion trajectory and control action sequence to the real-world representation of the traffic scenario.

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

[0100] Some embodiments of the present application are based on the recognition that by using a first transformation step to compute a mixed integer convex approximation of the vehicle decision and motion planning problem 330, real-world safety and traffic rules can be satisfied by the DM-MPS, including individual traffic actors and their prediction models, and including convex approximations of individual traffic rules using road-aligned coordinate systems. In some embodiments of the present application, convex approximations of individual traffic rules are used in the first transformation step 330 to conservatively satisfy individual safety and traffic rules in a real-world representation of the traffic scenario 345.

[0101] Some embodiments of the present application are based on the recognition that the first transformation step 330 allows for the 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 quadratic constraint quadratic programming (MIQCQP) problem, which can be efficiently solved, for example, using branch-and-bound optimization methods. In some embodiments of the present application, after computing a solution to the MICP 340 at each sampled time instant of the DM-MPS system, a second inverse transformation step is used to go from the approximated representation of the optimal motion trajectory and control action sequence using road-aligned coordinate systems to a real-world representation of the optimal motion trajectory 345, such that the transformed motion trajectory can be executed by the vehicle control system 325. In some embodiments of the present application, the vehicle control system is implemented using a model predictive controller (MPC) that is designed to follow the continuous reference trajectory computed by the DM-MPS module.

[0102] FIG. 3C A block diagram illustrating a transformation and solution strategy for computing an approximated value of the optimal motion trajectory and optimal action sequence of a controlled vehicle 360 according to some embodiments of the present application is shown, including relaxing one or more configuration parameters that cause non-convexity of the constrained optimization problem 355, and including tightening one or more limit parameters that are independent of the non-convexity of the constrained optimization problem for vehicle decision and motion planning 356. In some embodiments of the present application, a vehicle decision and motion planning system is based on the formulation and solution of the MICP problem, including relaxed configuration parameters 355 and tightened limit parameter values 356, to compute an approximated representation of the optimal motion trajectory and control action sequence 360, followed by a second inverse transformation step from the approximated representation of the optimal motion trajectory and control action sequence to a real-world representation of the traffic scenario 345.

[0103] Some embodiments are based on the recognition that various real-world traffic scenarios can be represented by a set of one or more parameters 350. Some parameters, such as the curvature of a road or the shape of a vehicle, can cause non-convexity in the constrained optimization problem for vehicle decision and motion planning. While other parameters, such as the limits on longitudinal velocity and / or acceleration and the limits on lateral velocity and / or acceleration, are irrelevant to non-convexity, i.e., they do not cause non-convexity in the optimization problem. For example, in some embodiments of the invention, a convex approximation of a nonlinear vehicle kinematic model is used that includes a limit on lateral velocity that depends on longitudinal velocity, thereby avoiding non-convexity in the optimization problem.

[0104] Some embodiments are based on the recognition that one or more configuration parameters 356 that cause non-convexity can be relaxed 355 at the cost of tightening one or more limit parameters 356 that are irrelevant to non-convexity. For example, in some embodiments of the invention, a configuration parameter for road curvature can be relaxed to make the road straight by tightening the limits on lateral velocity and / or lateral acceleration of the vehicle. In another example, in some embodiments of the invention, a parameter for a left or right turn at a traffic intersection can be relaxed to make the road straight by tightening the limits on lateral position of the vehicle and / or by tightening the limits on lateral velocity and / or lateral acceleration of the vehicle. Similarly, a parameter for the rate of turn limit of the vehicle can be relaxed to linearize the vehicle model by tightening the limits on lateral velocity and / or lateral acceleration of the vehicle. And in another example, in some embodiments of the invention, a parameter for the physical shape of the vehicle can be relaxed for collision avoidance constraints by tightening the limits on position of the vehicle. In this way, by relaxing a parameter of a current real-world traffic scenario that causes non-convexity in a mixed-integer non-convex constrained optimization problem and by tightening at least some parameters of the current real-world traffic scenario that are irrelevant to non-convexity in the mixed-integer non-convex constrained optimization problem, the mixed-integer non-convex constrained optimization problem can be converted to a mixed-integer convex constrained optimization problem.

[0105] In some embodiments of the invention, the proposed system and method for vehicle decision and motion planning uses a transformation of a real-world traffic scenario 350 at each control time step using a relaxation of configuration parameters 355 and a tightening of one or more limit parameters 356 in a mixed-integer convex programming (MICP) approximation representation 360 of a set of one or more parameters from the real-world traffic scenario. In some embodiments of the invention, the real-world traffic scenario of a vehicle motion planning problem of a vehicle driving on a curved road segment with one or more other vehicles is transformed to an approximation representation of a vehicle motion planning problem of a vehicle driving on a straight road segment with one or more other vehicles, which has tightened limit parameter values (e.g., tightened boundary values of one or more constraint functions in the MICP-based traffic scenario approximation representation), which contains a (piecewise) linear dynamics model of the (semi-)autonomous driving vehicle and a simplified representation of the vehicle’s environment in the traffic scenario. According to some embodiments of the invention, the MICP solution 360 comprises an optimal motion trajectory and a sequence of control actions of the vehicle, which is additionally transformed back from the approximation representation of the traffic scenario 345 to the real-world representation, thereby controlling the motion of the (semi-)autonomous vehicle in the real-world traffic scenario.

[0106] Some embodiments of the invention are based on the recognition that MICP problems are convex constrained optimization problems, which are computationally inexpensive to solve for a fixed set of values of each integer variable and, thus, can use computationally efficient branch-and-bound methods and / or machine learning-based techniques to efficiently compute a fixed set of values for each optimization variable in the optimal MICP solution.

[0107] FIG. 4A A transformation 415 of a vehicle decision and motion planning problem in an exemplary traffic scenario with one or more other vehicles on a curved road segment 410 to an approximation representation 420 of a vehicle motion planning problem with one or more other vehicles on a straight road segment 430 with tightened limit parameter values in a constrained mixed-integer convex optimization problem is shown according to some embodiments of the invention. More specifically, the real-world traffic scenario 400 comprises a curved road segment 410 with one or more lanes, a controlled vehicle 401, and one or more other vehicles 405. In some embodiments of the invention, a predicted motion trajectory 406 of each of the other vehicles from a current time step 405 to one or more future time steps 407 within a prediction time horizon can be computed.

[0108] In some embodiments of the invention, the predicted motion trajectories 406 are computed using a closed-loop kinematic vehicle model for lane keeping based on a modeling assumption that each of the other vehicles will remain in the current lane during the prediction time horizon. Some embodiments of the invention are based on the recognition that approximation errors in the prediction model for the other vehicles can be corrected based on a look-back time implementation of the vehicle decision and motion planning system. In some embodiments of the invention, the predicted motion trajectories 406 are computed using a data-based vehicle model that aims to predict the behavior of human drivers, e.g., using deep learning, support vector machines, neural networks, neural processes, Gaussian processes, and other machine learning algorithms or any combination of such deterministic and / or stochastic prediction models. According to some embodiments of the invention, the data-based vehicle model can include lane keeping, cruise control, stop, and / or lane change behavior modeling.

[0109] In some embodiments of the invention, the prediction model for one or more other vehicles takes into account interactions between the other vehicles, between the vehicle and the traffic environment (e.g., traffic lights at an intersection, speed limits), and / or between the controlled vehicle and one or more other vehicles, e.g., a vehicle deceleration can cause one or more other vehicles to decelerate, or a lane change by one vehicle can cause a reaction, e.g., one or more other vehicles to decelerate. In some embodiments of the invention, a reactive and / or interactive prediction model is implemented using a switched dynamical equation system that enables switching between different behavior modeling modes depending on the actions of the other vehicles and / or depending on changes in the traffic environment.

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

[0111] FIG. 4BAn inverse conversion 450 of an optimal solution of a mixed-integer convex optimization problem with tightening constraint parameter values to an approximate solution of a mixed-integer non-convex optimization problem of a real-world representation 460 on a curved road segment 410 of a vehicle decision and motion planning problem in an exemplary traffic scenario with one or more other vehicles is shown according to some embodiments of the present application. In some embodiments of the present application, the optimal solution of the MICP comprises an optimal motion trajectory 445 of the controlled vehicle 421 from the current time step 421 to one or more future time steps 446 within a prediction time horizon while avoiding any collisions with predicted motion trajectories 426 of 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 application, the optimal motion trajectory 445 comprises a sequence of vehicle positions, a sequence of orientation values, a sequence of longitudinal speed values, a sequence of acceleration and / or deceleration values, a sequence of engine and / or brake torque values, a sequence of steering values, and / or a sequence of one or more lane changes at future time steps to control the motion of the vehicle 421 in the approximate representation 440 of the real-world traffic scenario.

[0112] In some embodiments of the present application, the approximate motion trajectory 465 and control action sequence of the vehicle 401 from the current time step 401 to one or more future time steps 466 within a prediction time horizon while avoiding any collisions with predicted motion trajectories 406 of one or more other vehicles from the current time step 405 to one or more future time steps 407 within the prediction time horizon is computed in the real-world representation 460 of the traffic scenario using the inverse conversion step 450. Some embodiments of the present application are based on the recognition that the approximate motion trajectory 465 and corresponding control action sequence is computed using the inverse conversion step 450 to control the motion of the (semi-)autonomous vehicle 401 in the real-world traffic scenario. For example, in some embodiments of the present application, the approximate real-world motion trajectory 465 comprises a sequence of vehicle positions, a sequence of orientation values, a sequence of longitudinal speed values, a sequence of acceleration and / or deceleration values, a sequence of engine and / or brake torque values, a sequence of steering values, and / or a sequence of one or more lane changes at future time steps to control the motion of the vehicle 401 in the real-world traffic scenario 460.

[0113] In some embodiments of the present application, the vehicle decision and motion planning system includes motion prediction for one or more human-driven vehicles (HDVs) that uses switched dynamical systems to represent the HDV's reaction to possibly changing traffic rules. For example, if a traffic light for the desired direction of traffic for the HDV is red and the HDV is within a predetermined distance from a stop zone at an intersection, the following state-dependent switched dynamics can be used: the HDV stops at the traffic light at a particular intersection. Otherwise, if a leading vehicle is within a particular predetermined distance ahead of the HDV in the traffic network, the HDV follows the leading vehicle and maintains a safe following distance. Otherwise, the HDV drives at a desired target speed in the traffic scenario.

[0114] For example, the switched dynamical equations for computing the predicted motion trajectories of one or more other vehicles in a traffic scenario can be implemented using one or more optimization variables and one or more mixed-integer inequality and / or equality constraints in the MICP problem solved by the proposed vehicle decision and motion planning system.

[0115] FIG. 5 A conversion step 515 of a vehicle decision and motion planning problem in an exemplary traffic scenario with one or more other vehicles in the vicinity of a traffic intersection in the mixed-integer convex optimization problem according to some embodiments of the present application from a real-world representation 500 to an approximate representation 520 of the vehicle motion planning problem on a straight road segment with tightened limit parameter values, one or more exclusion zone constraints, and one or more anti-collision constraints is shown. Furthermore, FIG. 5 A reverse conversion 540 of the optimal solution of the mixed-integer convex optimization problem of the approximate representation 520 of the vehicle decision and motion planning problem in the exemplary traffic scenario with one or more other vehicles to an approximate solution of the mixed-integer non-convex optimization problem of the real-world representation 500 is shown.

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

[0117] In some embodiments of the application, the approximate representation of the vehicle motion planning problem with tightened limit parameter values 520 includes one or more zones, e.g., a first zone with only one lane, defined based on the exclusion zone constraint 535, followed by a second zone with two lanes, defined based on a different exclusion zone constraint 536, and potentially one or more additional zones including one or more mixed integer inequalities and / or equality constraints to impose traffic rules, e.g., speed limit constraints, lane change constraints, stop zone constraints, etc. Furthermore, in some embodiments of the application, the approximate representation 520 includes the controlled vehicle 521, one or more other vehicles 525a-525d, and / or a predicted motion trajectory for each of the other vehicles and / or traffic participants from the current time step to one or more future time steps in a prediction time horizon can be computed in the approximate representation 520.

[0118] In some embodiments of the application, the approximation representation 520 includes additional conflict zones 531 having one or more mixed-integer convex inequalities and / or equality constraints imposing rules in traffic intersections and / or junctions. For example, in some embodiments of the application, traffic light signals 510a-c in the real-world traffic scenario are implemented in the approximation representation 530 by imposing collision avoidance constraints on the conflict zones 531 at one or more future time steps in the prediction time horizon of the vehicle decision and motion planning system. For example, if a traffic light 530 is red for a particular time step in a particular intersection direction, the one or more conflict zone constraints 531 force the controlled vehicle 521 to stop in the approximation representation 520 before the traffic intersection, which corresponds to a stop maneuver in the real-world traffic scenario 500. Additionally, if the traffic light 530 is predicted to turn green at a future time step in the prediction time horizon, the one or more conflict zone constraints 531 are removed from the constrained mixed-integer convex optimization problem at the future time step in the prediction time horizon, thus computing an optimal motion plan for the controlled vehicle 521 to cross the traffic intersection.

[0119] In some embodiments of the application, the optimal solution of the MICP includes an optimal motion trajectory 522 of the controlled vehicle 521 from the current time step 521 to one or more future time steps 522 in the prediction time horizon and a corresponding sequence of control actions, while avoiding any collisions with the predicted motion trajectories of one or more other vehicles 525a-d, while avoiding any violations of the conflict zone constraints 531, and while obeying one or more additional zone constraints 535 and / or 536 imposing complex traffic rules in the local neighborhood along the particular route of the controlled vehicle 501 in the traffic network. In some embodiments of the application, the inverse transformation step 540 is used to compute an approximated motion trajectory 545 and a sequence of actions to safely and optimally control the (semi-) autonomous vehicle 501 in the real-world traffic scenario 500 from the current time step to one or more future time steps in the prediction time horizon, while avoiding any collisions with the predicted motion trajectories of one or more other vehicles 505a-d, and while avoiding any violations of complex traffic rules in the real-world representation 500 of the traffic scenario from the current time step to one or more future time steps in the prediction time horizon.

[0120] For example, in some embodiments of the application, the optimal motion trajectory 522 and / or the transformed motion trajectory 545 includes a sequence of vehicle positions, a sequence of orientation values, a sequence of longitudinal speed values, a sequence of acceleration and / or deceleration values, a sequence of engine and / or brake torque values, a sequence of steering values, and / or one or more lane change sequences at the future time steps to control the motion of the vehicle 501 in the real-world traffic scenario 500.

[0121] FIG. 6A A block diagram of a system and method for mixed integer model predictive control (MI-MPC) in the proposed vehicle decision and motion planning system according to some embodiments is shown to implement a predictive controller 110 that computes control signals 111 given the current state of the system 121 and control commands 101. Specifically, the MI-MPC computes control solutions, e.g., containing solution vectors 635 of future optimal discrete and continuous control input sequences over a prediction horizon of the system 640, by solving a constrained mixed integer convex programming (MICP) problem 630 at each control time step. The MICP data 625 of the objective function, equations, and discrete and continuous inequality constraints in this optimization problem 630 depend on one or more of the dynamic model of the controlled vehicle and traffic environment, system limits, and an approximate representation of the traffic scenario 620, the current state of the system 121, control objectives, and control commands 101.

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

[0123] In some embodiments, the mixed integer optimization algorithm is based on a search algorithm, such that the MI-MPC controller updates and stores additional mixed integer programming solution information 640 to reduce the computational effort of the search algorithm at the next control time step. In one embodiment, a branch-and-bound optimization method is used to solve the MI-MPC problem at each control time step, and the warm-start information 640 includes data related to the 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, thereby improving the node selection and variable branching strategy from one control time step to the next control time step.

[0124] FIG. 6BA block diagram of the MI-MPC method to solve an optimal control structured mixed integer linear quadratic optimization problem 650 is shown to compute control signals 111 at each control time step given the current state 121 of the vehicle 120 and commands 101 to compute a motion plan comprising a sequence of future discrete decisions and continuous actions for the controlled vehicle. Some embodiments of the present invention are based on a linear dynamics model of the vehicle and / or a linear dynamics 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 such that 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 and motion planning system. The MIQP data 625 then includes the Hessian and constraint Jacobian matrix 646 and the corresponding gradient and constraint evaluation vector 647. Typically, the linear discrete equality and / or integer feasibility constraints 655 express linear functions of state and control values E k x k +F k u k constrained to equal one of a discrete value set.

[0125] In some embodiments of the present invention, the linear discrete equality constraints 655 are binary equality constraints, e.g., E k x k +F k u k ∈{0,1} which includes one or more constraints that limit a control input variable or an auxiliary optimization variable to equal 0 or 1 at each time step in the prediction horizon in the optimal MIQP solution vector 635.

[0126] FIG. 6C A block diagram of the MIMPC method is shown that aims to find a feasible but possibly suboptimal solution vector 635 for an optimal control structured MICP 680 to compute control signals 111 at each control time step given current problem parameter values 675 and commands 101 to compute a motion plan comprising a sequence of future discrete decisions and continuous actions for the controlled vehicle. The optimal control structured MICP 680 includes minimizing or maximizing stage cost functions 681 g0(x0,u0,δ0;θ), g1(x1,u1,δ1;θ),…, g N-1 (x N-1 ,u N-1 ,δ N-1 ;θ) and a terminal cost function g N (x N ,uN ,δ N ; θ) that depends on state variables x 0:N = x0, x1,..., x N , continuous control input variables u 0:N = u0, u1,..., u N , discrete control variables δ 0:N = δ0, δ1,..., δ N , and problem parameter variables θ. In some embodiments of the invention, the state variables x 0:N are continuous optimization variables, the control input variables u 0:N are continuous optimization variables, and the discrete variables δ 0:N include binary and / or integer optimization variables.

[0127] In some embodiments of the invention, the constraints of the optimal control structured MICP 680 include equality constraints on initial state variables x0= x init (θ) 682, where the current state values of the system can be computed by the state estimator 130 and can depend on one or more problem parameters in the traffic scenario. In some embodiments of the invention, one or more initial state variables are free and the corresponding equality constraints 682 can be omitted. In some embodiments of the invention, the constraints of the optimal control structured MICP 680 include a sequence of equality constraints to define the state variables at a next time step based on the state and control variables at previous time steps in the prediction horizon, e.g., a sequence of equality constraints x1= ψ0(x0, u0, δ0; θ), x2= ψ1(x1, u1, δ1; θ),..., x N = ψ N-1 (x N-1 , u N-1 , δ N-1 ; θ) 683 based on the dynamic model 102 of the controlled vehicle and its traffic environment. In some embodiments of the invention, the constraints of the optimal control structured MICP 680 include a sequence of inequality constraints on one or more combinations of state and / or control variables at various time steps in the prediction horizon, e.g., a sequence of inequality constraints and where n f is the number of inequality constraints at the various time steps.

[0128] In some embodiments of the invention, one or more of the discrete variables are binary optimization variables, i.e., for i = 0, 1,..., N and j in the index set of binary optimization variables, δ i,j{0, 1}685. In other embodiments of the application, one or more of the discrete variables are integer optimization variables, i.e., for i = 0, 1,..., N and j, Some embodiments of the application are based on the recognition that if the MICP function 627 is convex, i.e., the stage cost function g i (·)681 is convex, the dynamic model function ψ i (·)683 is linear or piecewise linear, and the inequality constraint function f i,j (·)684 is convex, then the optimal control structured MICP 680 can be solved relatively efficiently.

[0129] Some embodiments of the application are based on the recognition that if the stage cost function g i (·)681 is linear, the dynamic model function ψ i (·)683 is linear or piecewise linear, and the inequality constraint function f i,j (·)684 is linear or piecewise linear, then the optimal control structured MICP 680 can be reformulated as a mixed integer convex linear program (MILP). Some embodiments of the application are based on the recognition that if the stage cost function g i (·)681 is linearly quadratic, the dynamic model function ψ i (·)683 is linear or piecewise linear, and the inequality constraint function f i,j (·)684 is linear or piecewise linear, then the optimal control structured MICP 680 can be reformulated as a mixed integer convex quadratic program (MIQP).

[0130] In some embodiments of the application, one or more of the inequality constraint functions f i,j (·)684 are convex quadratic inequalities, such that the optimal control structured MICP 680 can be reformulated as a mixed integer quadratic constraint quadratic program (MIQCQP). In some embodiments of the application, one or more of the inequality constraint functions f i,j (·)684 are convex second-order cone constraints, such that the optimal control structured MICP 680 can be reformulated as a mixed integer second-order cone program (MISOCP). Some embodiments of the application are based on the recognition that MICP problems, including

[0131] MILP, MIQP, MIQCQP, or MISOCP can be solved relatively efficiently, for example, using a branch-and-bound method, a branch-and-cut method, a branch-and-price method, or any other variant of a tree-search based optimization algorithm. However, some embodiments of the present application are based on the recognition that the combined complexity of MICP typically leads to an exponential increase in computation time for optimization algorithms to solve MICP with increasing numbers of discrete decision variables, such that in some embodiments of the present application, a feasible but suboptimal solution vector 635 is computed at reduced computational cost. In some embodiments of the present application, the proposed vehicle decision and motion planning system solves the optimal control structured MIQP 50 for each sampling time step in the following form:

[0132]

[0133] A i x i +B i u i +a i =x i+1 , i e {0,..., N - 1}, (1c)

[0134] State boundary:

[0135]

[0136] 0 < Y ref (i) < (n - 1)d, i e {0,..., N}, (1e)

[0137]

[0138] 0 < t c (i) < t D , i e {0,..., N}, (1g)

[0139] Control boundary:

[0140]

[0141] Prohibit vehicle motion:

[0142] a | v Y (i) | - v X (i) < 0, i e {0,..., N - 1}, (1k)

[0143] Lane change:

[0144]

[0145] - d(l u (i) + l d (i)) < δc (i) < d(i) + dl(i), i e {0,..., N}, (1i) u (i) + dl(i) d (i), i e {0,..., N}, (1m)

[0146] -d + 2dl u (i) < d(i) - dl(i) c (i) < d(i) - 2dl(i) d (i), i e {0,..., N}, (1n)

[0147] l d (i) e {0, 1}, i e {0,..., N}, (1o) u (i) e {0, 1}, i e {0,..., N}, (1o)

[0148] Time delay count:

[0149]

[0150] t D l u (i) + t D l d (i) < (t D - t c (i), i e {0,..., N}, (1q)

[0151] Obstacle constraint:... (see FIG. 7A to FIG. 7C ) (1r)

[0152] Region constraint:... (see FIG. 7D ) (1s)

[0153] where parameter d represents a lane width value, and parameter t D represents a time delay value defining a minimum amount of time elapsed between two consecutive lane changes. The above mixed integer optimal control problem formulation uses multiple continuous and / or discrete optimization variables, including a sequence of state variables X = [x0, x1,..., x N ] over a prediction time horizon of length N, a sequence of auxiliary variables Z = [z0, z1,..., z N ] and a sequence of control input variables U = [u0, u1,..., u N-1 ].

[0154] In some embodiments of the invention, the state variables of the optimal control structured MICP are defined as x i = [p X (i), p Y (i), v X (i), Y ref (i), n LC (i), tc (i), p X (i), p Y (i) denotes the position of the controlled vehicle in the global coordinate system aligned with the road, v X (i) denotes the longitudinal velocity, Y ref (i) denotes the preferred lane of the controlled vehicle, n LC (i) denotes the total number of performed lane changes, and t c (i) denotes a timer variable for the time since the previous lane change at time step t i (i) within the prediction time horizon. For example, if Y ref = (j - 1)d, given a lane width value d, for j = 1, 2,..., n, then the controlled vehicle is in lane j, where n is the number of lanes in the real-world traffic scenario, and n LC (i) denotes the total number of performed lane changes, and t i (i) denotes the total number of performed lane changes, and t

[0155] In some embodiments of the application, the input variables of the optimal control structure MICP are defined as where a X (i) denotes the longitudinal acceleration / deceleration of the controlled vehicle, v Y (i) denotes the lateral velocity of the controlled vehicle, denotes a continuous input variable for the lane change timer constraint, δ c (i) denotes a continuous variable defining the magnitude of the lane change according to the lateral position change, and the binary variable l u (i), l d (i), l i (i) denotes whether an upward or downward lane change is performed at future time steps t u (i), l d (i) e {0, 1} are binary optimization variables in the MIQP 650, while the input variable is a continuous optimization variable in the MIQP 650, solving for which the motion plan and sequence of control actions for the vehicle in the traffic scenario can be computed.

[0156] In some embodiments of the application, the linear dynamics model of the controlled vehicle in the approximate representation of the real-world traffic scenario, including the simplified vehicle dynamics and additional linear dynamics for implementation of traffic rules related to lane changes, for example, is as follows:

[0157]

[0158] Some embodiments of the invention are based on the recognition that the linear quadratic objective in the MIQP 650 aims at maximizing the travel distance, minimizing the number of lane changes, minimizing the cumulative amount of acceleration / deceleration values, and minimizing the lane error where is a user-defined value for the preferred lane of a road segment in the transportation network, such that the controlled vehicle aims to travel in the preferred lane as much as possible. For example, in some embodiments of the invention, the objective in the MICP 630 includes the minimization of the auxiliary control variable Ay ref , e.g., satisfying the following additional inequality constraint:

[0159]

[0160] such that the condition holds.

[0161] In some embodiments of the invention, the constrained MICP 630 includes a state variable t c (i) that represents the waiting time from the previous lane change until a new lane change can be initiated at a future time step in the prediction time window. For example, in some embodiments of the invention, a counter starts from a fixed total waiting time t D at the time of the lane change, and it counts down until a new lane change can be initiated at a future time step t i when t c (i) = 0. In some embodiments of the invention, an additional control variable is introduced to implement the following three options:

[0162] At the time step of the lane change t i , i.e., l u (i) = 1 or l d (i) = 1, then and t c (i+1) = t D - 1, because of the equality constraint

[0163] As long as the condition t c (i) > 1 holds, the controlled vehicle is not allowed to make a new lane change, and such that i.e., the counter variable t c (i) counts down from one time step to the next time step within the prediction time horizon.

[0164] If the condition t c (i) = 0 holds at the time step ti If true, allow the controlled vehicle to make a new lane change such that Option 1 above repeats itself or such that

[0165] In some embodiments of the present application, auxiliary binary optimization variables are used to implement one or more obstacle avoidance and / or space-dependent zone constraints as FIG. 7A to FIG. 7D shown.

[0166] Some embodiments of the present application are based on the recognition that the MICP 630 includes one or more obstacle avoidance constraints, which are typically time-varying, and can be used to implement collision avoidance of the controlled vehicle with respect to other static or dynamic vehicles, bicycles, or pedestrians, and also to implement traffic rules at stop signs or traffic lights at traffic intersections along the controlled vehicle’s specific route in the traffic network. The latter allows the same MICP formulation to be used to model approximate representations of many different real-world traffic scenarios.

[0167] FIG. 7A One or more obstacle avoidance constraints are shown using mixed-integer inequality constraints that force the controlled vehicle to be within one of a number of disjoint regions outside a safety region 700 around an obstacle 705, where the obstacle can be another vehicle, a bicycle, a pedestrian, a stop zone, or a traffic intersection in the traffic network. For example, in some embodiments of the present application, for each disjoint region outside the safety region 700 around the obstacle 705, an auxiliary binary variable FIG. 7A is introduced 711, 712, 713, and 714. The resulting set of mixed-integer inequality constraints can be defined for each obstacle, for example, as follows for j = 1, 2, …, n obs :

[0168]

[0169]

[0170] where M > 0 is a large positive value. In some embodiments of the present application, the objective in the MICP 630 includes the sum of the binary variables minimized, which results in a tight MICP formulation.

[0171] For example, because M > 0 is a large positive value, the mixed-integer inequality constraint implements the following implication:

[0172]

[0173] Alternatively, The following other implications are implemented:

[0174]

[0175] FIG. 7B One or more obstacle avoidance constraints for the controlled vehicle 721 in an approximate representation of an example real-world traffic scenario with one or more other vehicles 725a-725b are shown in the proposed vehicle decision and motion planning system. For example, in some embodiments of the invention, the one or more obstacle avoidance constraints force the controlled vehicle 721 outside of a safety region 730a around vehicle 725a and outside of a safety region 730b around vehicle 725b. In some embodiments of the invention, the controlled vehicle 721 can be forced outside of the safety region 730a at the current time step based on a predicted motion trajectory 726 of the vehicle from the current time step 725a to one or more future time steps 728, and the vehicle 721 can be forced outside of one or more other safety regions 732 around a predicted position and orientation of the vehicle 728 at one or more future time steps in the prediction time horizon of the vehicle decision and motion planning system. Some embodiments of the invention are based on the recognition that obstacle avoidance constraints outside of safety regions around predicted motion trajectories of individual obstacles result in a set of time-varying mixed-integer inequality constraints in the MICP 630 that are solved at individual control time steps.

[0176] FIG. 7C One or more obstacle avoidance constraints for the controlled vehicle 721 in an approximate representation of an example real-world traffic scenario with one or more other vehicles 725a-725b, corresponding safety regions 730a-730b, and one or more traffic intersections 740 along a particular route of the controlled vehicle 721 in a traffic network are shown in the proposed vehicle decision and motion planning system. Some embodiments of the invention are based on the recognition that a set of mixed-integer inequality constraints can be used to force the controlled vehicle 721 to remain outside of safety regions 745 around individual traffic intersections 740 at particular time steps in the prediction time horizon of the vehicle decision and motion planning system.

[0177] For example, if a traffic light is red for the desired cross direction at the current time step, one or more collision avoidance constraints force the controlled vehicle 721 to stop before entering a safety zone 745 around the traffic intersection 740, which corresponds to a stop maneuver in a real-world traffic scenario. In some embodiments of the application, the collision avoidance constraints for the safety zone 745 around the traffic intersection 740 are referred to as conflict zone constraints. For example, if a traffic light is predicted to turn green for the desired cross direction at a future time step in the prediction time horizon, 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 in the prediction time horizon to compute an optimal motion plan for the controlled vehicle 721 to cross the traffic intersection 740 in the desired cross direction.

[0178] In some embodiments of the application, complex traffic rules on the order of precedence in which vehicles can enter a traffic intersection without traffic lights can be implemented in a similar fashion, e.g., by imposing one or more conflict zone constraints at specific time steps in the prediction time horizon and relaxing and / or removing the conflict zone constraints at other time steps in the prediction time horizon. For example, in some embodiments of the application, the controlled vehicle 721 is only allowed to enter the safety zone 745 around the traffic intersection 740 after coming to a complete stop within a stop zone in front of the traffic intersection and only when no other vehicles are crossing the traffic intersection in a cross direction that can lead to a potential collision.

[0179] FIG. 7D One or more spatially dependent zone constraints 761-762 in the proposed vehicle decision and motion planning system for the controlled vehicle 751 in an approximate representation of an example real-world traffic scenario with one or more other vehicles 752-753 are shown. Some embodiments of the application 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 application, the spatially dependent zone constraints include

[0180] a limit on the maximum number of lane changes allowed for the controlled vehicle 751 in a particular spatially dependent zone, e.g., no lane changes are allowed when crossing a traffic intersection.

[0181] a constraint on the lanes allowed for the controlled vehicle 751 to travel in a particular spatially dependent zone of a road segment, e.g., the vehicle needs to merge into a lane before a particular point along the route of the controlled vehicle 751 in the traffic network.

[0182] a speed limit constraint that depends on the spatially dependent zone, e.g., the speed limit value generally decreases when the controlled vehicle 751 enters a busy urban street.

[0183] In some embodiments of the application, the MICP 630 includes auxiliary binary variables at each time step in the prediction horizon where n s represents the number of spatially dependent zones. For example, each zone is defined by a series of longitudinal positions of two values for each zone in the p X directions j = 1, 2,..., n s In some embodiments of the application, the MICP 630 includes mixed integer inequality constraints to define whether the controlled vehicle 751 is in one zone 761 or another zone 762, for example,

[0184]

[0185] Implement the following meaning:

[0186]

[0187] Since the spatially dependent zones are disjoint regions, the controlled vehicle 751 is forced to be in exactly one zone at each time step t i i.e.

[0188] Some embodiments of the application are based on the recognition that one or more auxiliary binary variables can be used to adapt complex traffic rules applicable along the route of the controlled vehicle 751 in the traffic network. For example, in some embodiments, speed limit constraints can be adapted to one or more spatially dependent zones as follows:

[0189]

[0190] where, represents the speed limit value in each zone j = 1, 2,..., n s Similarly, in some embodiments of the application, the boundaries on drivable lanes can be adapted to one or more spatially dependent zones as follows:

[0191]

[0192] where, and represent the boundaries on drivable lanes for each zone j = 1, 2,..., n sThe lower and upper bounds of the preferred lane of the controlled vehicle 751, parameter n represents the number of lanes in the real-world traffic scenario, and d represents the lane width value. And similarly, in some embodiments of the present application, the upper bound of the total number of lane changes of the controlled vehicle 751 can be applied to one or more spatially dependent zones as follows:

[0193]

[0194] wherein, represents the limit on the total number of lane changes of the controlled vehicle 751 in each zone, j = 1, 2,..., n s .

[0195] FIG. 8A A schematic diagram showing an example of a binary control variable search tree, representing a nested search zone tree for an integer feasible control solution, is shown in accordance with some embodiments. FIG. 8A A schematic diagram of a branch and bound method used to implement the MI-MPC controller in some embodiments is shown and represented by showing a binary search tree 800 of a mixed integer optimization algorithm at a particular iteration. The main idea of the branch and bound (B&B) method is to successively partition the original problem and then attempt to solve these partitions, where each partition corresponds to a particular zone of the discrete control variable search space. In some embodiments, the branch and bound method selects a partition or node and selects a discrete control variable to branch the partition into smaller partitions or search zones, thereby generating a nested tree of partitions or search zones.

[0196] For example, a partition P1 801 represents a discrete search zone that can be split or branched into two smaller partitions or zones P2 802 and P3 803, i.e. a first zone and a second zone nested in the common zone. The first and second zones are disjoint, i.e. the intersection of the two zones is empty P2∩P3= φ 807, but they collectively make up the original partition or zone P1, i.e. the union after branching P2∪P3=P1 806 holds. The branch and bound method then solves the integer relaxed MPC problem for the first and second partitions or zones of the search space, thereby obtaining two solutions (local optimal solutions) that can be compared to each other and to the current known upper bound on the optimal objective value. If the performance metric of the first and / or second partition or zone is lower than the current known upper bound on the optimal objective value of the MI-MPC problem, it can be pruned. If either the first zone, the second zone, or both zones generate a discrete feasible solution for the MI-MPC problem, the upper bound can be updated. The branch and bound method then continues to select the remaining zones in the current nested zone tree for further partitioning.

[0197] While solving individual partitions remains challenging, obtaining a local lower bound of the optimal objective value is quite effective through solving a local relaxation of the mixed integer program or exploiting duality. If the MI-MPC solver happens to obtain an integer feasible solution while solving the local relaxation, the MI-MPC solver can use it to obtain a global upper bound of the mixed integer control solution of the original MI-MPC problem. This helps to avoid solving or branching certain partitions that have already been created, i.e., these partitions or nodes can be pruned. The general algorithmic idea of such a partitioning can be represented as a binary search tree 800, including root nodes (e.g., P1 801 at the top of the tree) and leaf nodes (e.g., P4 804 and P5 805 at the bottom of the tree). Furthermore, nodes P2 802 and P3 803 are commonly referred to as direct children of node P1 801, and node P1 801 is referred to as the parent node of nodes P2 802 and P3 803. Similarly, nodes P4 804 and P5 805 are children of their parent node P2 802.

[0198] FIG. 8B A block diagram of a branch-and-bound mixed integer optimization algorithm is shown according to some embodiments, which searches for an integer feasible optimal control solution based on a nested search region tree and corresponding lower / upper bound values. FIG. 8B The block diagram of the branch-and-bound mixed integer optimization algorithm shown can be used to solve the MICP problem in some embodiments of the present application. The branch-and-bound method initializes the branch-and-search tree information of a mixed integer quadratic program (MIQP) at the current control time step 810 based on the MIQP data 625 composed of the MIQP matrix 646 and the MIQP vector 647. The initialization process can also use the branch-and-search tree information and the MIQP solution information from the previous control time step 610 in order to generate a warm start initialization for the current control time step 810. The main objective of the optimization algorithm is to construct upper and lower bounds of the mixed integer control solution objective value. If the gap between the upper and lower bound values is less than a certain tolerance value in step 811, the mixed integer optimal control solution 635 is found.

[0199] Some embodiments of the present application are based on the recognition that as long as the gap between the lower and upper bound values in step 811 is greater than a certain tolerance value and the optimization algorithm has not reached the maximum execution time, the branch-and-bound method will continue to iteratively search for the mixed integer optimal control solution 635. Each iteration of the branch-and-bound method starts with the selection of the next node in the tree, which corresponds to the next region or partition of the integer variable search space, and determines the possible variables based on the pre-solve branching technique 815. After the node selection, the corresponding integer relaxed MPC problem is solved and the possible variables are determined based on the post-solve branching technique 820.

[0200] Some embodiments of the present application are based on the recognition that if the integer relaxed MPC problem has a feasible solution, then the resulting relaxed control solution will provide a lower bound on the objective value for that particular region or partition in the integer variable search space. In step 821, if 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-and-bound tree 840. However, in step 821, if it is determined that the objective is lower than the currently known upper bound and the relaxed control solution is an integer feasible solution 825, then the currently known upper bound and the corresponding mixed-integer control solution estimate are updated in step 830 of the branch-and-bound optimization algorithm.

[0201] Some embodiments of the present application are based on the recognition that if the 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 on the objective can be updated 835 to the minimum of the objective values of the remaining leaf nodes in the branch-and-bound tree and the selected node is pruned from the tree 840. Furthermore, starting from the current node, a discrete variable with a fractional value is selected for branching 845 according to a particular branching strategy to create and append the resulting sub-problems corresponding to a region or partition of the discrete search space as child nodes of this node in the branch-and-bound tree 850.

[0202] An important step in the branch-and-bound method is how to create the partitions, i.e., which node 815 to select and which discrete variable to select for branching 845. Some embodiments are based on branching on one of the binary control variables with a fractional value in the integer relaxed MPC solution. For example, if a particular binary control variable u i,k ∈ {0, 1} has a fractional value as part of the integer relaxed MPC solution, then some embodiments create two partitions of the mixed-integer program by adding the equality constraint u i,k = 0 to one sub-problem and the equality constraint u i,k = 1 to the other sub-problem. Some embodiments are based on a reliability branching strategy for variable selection 845 that aims to predict future branching behavior based on information from previous branching decisions.

[0203] Some embodiments are based on a branch-and-bound method that uses a depth-first node selection strategy, 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 the process is repeated until a node is pruned, i.e., the node is infeasible, optimal, or dominated by a currently known upper bound, followed by a backtracking process. In contrast, some embodiments are based on a branch-and-bound method that uses a best-first strategy that selects the node with the current lowest local lower bound. Some embodiments employ a combination of depth-first and best-first node selection methods, where the depth-first node selection strategy is used until an integer-feasible control solution is found, and then the best-first node selection strategy is used in subsequent iterations of the branch-and-bound based optimization algorithm. The motivation for the latter implementation is to find an integer-feasible control solution early in the beginning of the branch-and-bound process (depth-first) to allow early pruning, and then to search for a better feasible solution more greedily (best-first).

[0204] The branch-and-bound method continues to iterate until one or more of the following conditions are met.

[0205] The maximum execution time of the processor is reached.

[0206] All nodes in the branch search tree have been pruned such that no new nodes can be selected for convex relaxation or branching for the solution of the problem.

[0207] The optimality gap between the global lower bound value and the upper bound value for the purpose of the mixed-integer control solution is less than a tolerance.

[0208] FIG. 9A It is shown that the optimal control structured MICP problem 970 can be converted and solved 630 into an optimal control structured convex program (CP) 980 after fixing all discrete variables to a fixed value set 975, i.e., after fixing all binary variables to 0 or 1 and / or fixing all integer variables to integer-feasible values. Some embodiments of the present invention are based on the recognition that after fixing the discrete variables to the optimal discrete solution that can be obtained by solving the original MICP 970 The optimal state and control solution of the optimal control structured CP 980 is equal to the optimal state and control solution of the optimal control structured CP 970 to compute the solution vector 635. Thus, when fixing the discrete optimization variables 975, by accurately predicting the optimal discrete solution It is possible to solve the CP 980 instead of solving 630 the MICP problem 970, which is computationally much cheaper to solve. In some embodiments of the present invention, the fixed value set of the discrete variables 975 is computed by a machine learning based prediction and an iterative pre-solution based correction method to compute a feasible but possibly suboptimal solution vector 635.

[0209] The optimal control structured CP 980 is based on minimization or maximization of a sum of stage cost functions 981, and constraints can include MICP functions 627 g0, g1,..., g N , ψ0, ψ1,..., ψ N-1 , and equality constraints on initial state variables 982, state dynamic equality constraints 983, and one or more inequality constraints 984, where for i = 0,..., n and j = 1,..., n δ each discrete optimization variable δ i,j is fixed to a predicted value 975. Note that after fixing the discrete variables 975, the discrete optimization variables δ 0:N and the discrete feasibility constraints 685 are removed from the optimal control structured CP 980. In some embodiments of the present application, the optimal control structured CP can be efficiently solved using convex optimization algorithms including, for example, active-set methods, interior-point methods, gradient-based methods, operator-splitting methods, or the Alternating Direction Method of Multipliers (ADMM).

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

[0211] FIG. 9BA block diagram of the MICP solution method 630 is shown that, given current problem parameter values θ675, computes a feasible but potentially suboptimal solution vector 635 defining the control signals 111 based on the solution of a structured CP approximation 980 of the prediction and optimal control of a fixed value set of the discrete optimization variables 905. In some embodiments of the present invention, given the current problem parameter values θ675, the prediction of the fixed value set of the discrete optimization variables 905 is based on a parametric function approximation of the mapping between the problem parameters θ675 and the discrete values of the optimal MICP solution 630. Some embodiments of the present invention are based on the recognition that if all discrete optimization variables are binary optimization variables, then the discrete values of the optimal MICP solution 630 are equal to 0 or 1. In some embodiments of the present invention, the prediction method 905 is based on a parametric function approximation of the mapping between the problem parameters θ675 and the values or for each of the binary optimization variables in the optimal MICP solution 630, i = 0, 1,..., N and j = 1, 2,..., n δ .

[0212] In some embodiments of the present invention, the prediction model 905 can be deterministic, e.g., including multilayer 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 prediction models. Alternatively, in some embodiments of the present invention, the prediction model can be stochastic, e.g., including Bayesian neural networks, neural processes, Gaussian processes, Kriging interpolation, and other machine learning algorithms or any combination of such deterministic and / or stochastic prediction models.

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

[0214] In some embodiments of the present invention, the prediction 905 of a fixed set of values ​​for the discrete optimization variables is followed by a correction of the fixed set of values ​​for the discrete optimization variables to increase the likelihood of feasibility and optimality 910, followed by a solution of the optimal control structured CP approximation 980 to compute a feasible but potentially suboptimal solution vector 635 defining the control signal 111. For the binary optimization variables in the optimal MICP solution 630, the prediction method 905 can be implemented as a problem parameter θ 675 and a value or The parameter function approximation of the mapping between i = 0, 1, ..., N and j = 1, 2, ..., n δ However, some embodiments of the present invention are based on the recognition that for the solution vector 635 calculated from the solution of the CP approximation 980, the predicted value or 905 may result in a relatively low likelihood of feasibility and / or optimality in practice.

[0215] In some embodiments of the invention, if a fixed set of values ​​for the discrete optimization variables results in a feasible and / or optimal CP solution 980 for the original MICP optimization problem 970, the correction method 910 does not adjust the predicted values. or 905. When the resulting convex optimization problem (CP) has a feasible optimal solution, the value set of the discrete variable Alternatively, if the CP problem 980 is infeasible after fixing the discrete optimization variables to the set of predicted values ​​905, the correction method can adjust one or more of the predicted values ​​to better approximate the optimal discrete solution for each of the binary optimization variables in the optimal MICP solution 630. And j=1,2,…,n δ , and thus increase the likelihood of feasibility and optimality 910 according to embodiments of the present invention.

[0216] Finally, the updated discrete value set For fixing each discrete optimization variable 910, the resulting CP 980 can be efficiently solved to compute a feasible optimal control solution vector 635, which includes trajectories of the values ​​of the state and control input variables at each time step of the prediction time horizon, which can be (close to) the optimal control solution of the original MIOCP 970 for computing the motion plan and control action sequence of the vehicle in the traffic scene.

[0217] In some embodiments of the application, the correction method 910 is an iterative pre-solve based correction method that is based on an iterative process in which a single pre-solve step is performed at each iteration, followed by fixing one or more discrete optimization variables to a set of predicted fixed values 905. In some embodiments of the application, the single pre-solve step itself consists of an iterative process of one or more pre-solve operations to reduce the number of discrete and / or continuous optimization variables, tighten the bounds of the optimization variables, and / or tighten the bounds of one or more inequality constraints. The iterative process of the single pre-solve step then performs one or more iterations until the problem is detected as infeasible, until not enough progress is made between two consecutive iterations, or until a predetermined time limit has been reached to ensure computational efficiency. Examples of pre-solve operations include domain propagation, bound tightening, dual fixing, implicit variable substitution, coefficient tightening, probing, detection, and removal of redundant variables and / or constraints. Additionally, individual pre-solve operations can be performed on a single variable, a single constraint, multiple variables, and / or multiple constraints.

[0218] FIG. 9C A block diagram of the MICP solution method 630 is shown, which is given a current problem parameter value θ 675, a plurality of different predictions 921-922 of the fixed value set of the discrete optimization variables, each prediction followed by a correction step 931-932 of the fixed value set of the discrete optimization variables to increase the likelihood of feasibility and optimality, and each prediction followed by a solution of the optimal control structured CP approximation 941-942 to compute a feasible but possibly suboptimal solution vector 635 by selecting the feasible control solution with the optimal optimality from the CP solution of each of the n corrected discrete solution predictions 950. In some embodiments, the computation of each discrete solution prediction can be performed in parallel. For example, the first prediction of the discrete solution guess 921 can be computed in parallel with one or more other predictions of the discrete solution guess 922. Similarly, the correction step for the first prediction of the discrete solution guess 931 can be computed in parallel with one or more correction steps for other predictions 932. Finally, the optimal control structured CP solution for the fixed 941 can be computed in parallel with one or more CP solutions for the fixed value set 942, thereby selecting the optimal optimal control solution 950 to compute the MICP solution vector 635.

[0219] FIG. 9D ​​​​​​A block diagram of the MICP solution method 630 is shown that, given current problem parameter values θ675, makes a prediction based on a fixed set of values of the discrete optimization variables 905, which is used to fix a first subset of the discrete variables in the MICP to predicted values output by the trained parameter function 911, followed by a pre-solution correction to fix a remaining subset of the discrete variables to values that are uniquely defined by the fixed values of the first subset of the discrete variables and based on the constraints in the MICP 912 to increase the likelihood of feasibility and optimality of the updated fixed set of values of the discrete variables 910, and followed by a solution of the optimal control structured CP approximation 980 to compute a feasible, but possibly suboptimal, solution vector 635 that defines the control signals 111.

[0220] FIG. 10A An offline data generation and supervised learning process 1000 for training a machine learning based predictor 1015 and then used in an online variable fixing and optimal control solution process 1020 to compute a solution vector 635 for optimal control structured MICP is shown according to some embodiments of the present application, and a block diagram of the machine learning based predictor then used in an online variable fixing and optimal control solution process 1020 to compute a solution vector 635 for optimal control structured MICP. The offline data generation and supervised learning process 1000 includes a first step of sampling problem parameter values θ 1 ,θ 2 ,...,θ M 1005, which represent the MICP problem that needs to be solved online in the proposed real-time vehicle decision and motion planning system. For example, if the problem parameters include the current state of the controlled vehicle and / or the traffic environment, the data sampling step 1005 ensures a sufficiently large number of sampled parameter values lie in one or more regions of the feasible state space of the vehicle and its traffic environment. Additionally, if the problem parameters include a representation of one or more obstacles, the data sampling step 1005 ensures a sufficiently large number of sampled parameter values result in varying positions, orientations, and / or dimensions of the one or more obstacles in the traffic environment.

[0221] The offline data generation and supervised learning process 1000 includes a second step of solving the MICP to compute optimal control solutions (x 1 ,θ 2 ,...,θ M 1005, for each sampled problem parameter value θ ＊ ,θ ＊ ,θ ＊)1010. In some embodiments of the application, each MICP 1010 can be solved exactly, e.g., using a branch-and-bound method (B&B), a branch-and-cut method, a branch-and-price method, or any other variant of a tree-search based optimization algorithm. Alternatively, in some embodiments of the application, each MICP 1010 can be solved approximately, e.g., based on a rounding and pumping scheme, using an approximate optimization algorithm, approximate dynamic programming, or using sequential convex programming (SCP) techniques.

[0222] The offline data generation and supervised learning process 1000 comprises a third step of training a machine learning network using supervised learning based on the training data set of sampled parameter values 1005 and the corresponding MICP solutions 1010 for the purpose of machine learning based prediction of the discrete optimal solution δ ＊ for a given new set of problem parameter values 1015. In some embodiments of the application, the training of the machine learning network involves the computation of network weights φ that minimize a training loss function, e.g., using stochastic gradient descent, Adam, AdaGrad, RMSProp, or any other variant of a gradient or gradient-free optimization algorithm.

[0223] The online variable fixing and optimal control solution process 1020 comprises a first step of evaluating the machine learning based predictor to compute a discrete solution guess 1030 given the current problem parameter values θ 1025. Given the predicted discrete solution guess 1030, the online variable fixing and optimal control solution process 1020 comprises a second step of performing a correction of the discrete solution guess, resulting in an updated solution guess 1035 to improve feasibility and / or optimality. Finally, a third step is based on the solution of the optimal control structured CP for the fixed discrete values 1040. In some embodiments of the application, the correction method 1035 is an iterative pre-solve based correction method that is based on an iterative process where each iteration performs a single pre-solve step followed by fixing one or more discrete optimization variables to the set of predicted fixed values 1030.

[0224] Some embodiments of the application are based on the recognition that the offline data generation and supervised learning process 1000 can be executed offline on a high-performance computer and, therefore, it has no strict requirements on computational complexity and worst-case computation time. On the other hand, embodiments of the application are based on the recognition that the online variable fixing and optimal control solution process 1020 needs to be executed online, has strict requirements on computational complexity and worst-case computation time, and the online process is typically executed on an embedded microprocessor with limited memory and limited computational power, e.g., using an embedded control unit (ECU) of a vehicle.

[0225] FIG. 10B A flowchart is shown of an offline data generation process 1000 based on a MICP solution 1010 for a set of sampled problem parameter values ​​1005 and a supervised learning process for training a machine learning-based predictor 1015 based on a discrete optimizer set Δ 1053 and a training dataset D 1054. In some embodiments of the present invention, offline data generation 1000 is an iterative process that checks whether the training dataset D is sufficiently large 1055. If the training dataset D is sufficiently large 1055, the process initiates training of the machine learning-based predictor 1015, i.e., computing the network weights φ that minimize the training loss function using stochastic gradient descent, Adam, AdaGrad, RMSProp, or any other variant of a gradient-based or gradient-free optimization algorithm. In some embodiments of the present invention, the offline data generation process 1000 returns the trained machine learning network H. φ and discrete optimizer set Δ1056.

[0226] If the training dataset D is not sufficiently large 1055, then data generation 1000 is performed by calculating new sampling problem parameter values ​​θ k 1050 and solve the MICP problem to calculate the parameter value θ for the new sampling problem k The optimal control solution (x k,＊ ,u k,＊ ,δ k,＊ ) 1051. If the MICP solution 1051 is not feasible 1052, then it is necessary to calculate new sampling problem parameter values ​​θ k 1050. In some embodiments of the present invention, a batch of pre-calculated problem parameter values ​​{θ k} k＝1,...,M 1045 Select a new sampling problem parameter value θ k 1050. At the start of the offline data generation process 1000, the binary optimizer set Δ and the training dataset D are each initialized to be equal to the empty set 1001.

[0227] If the MICP solution 1051 is feasible 1052, then data generation 1000 is performed by converting the optimal discrete solution value Add to the discrete optimizer set Δ to perform, and for each time step i=0, ..., N in the prediction time range 1053 the optimal discrete solution value Identifying unique class labels In some embodiments of the present invention, if the MICP solution 1051 is feasible 1052, the offline data generation 1000 is performed by sampling the solution values ​​at each time step i=0, ..., N. Add to training dataset D 1054 to proceed.

[0228] Some embodiments of the present application are based on the recognition that two or more sampled problem parameter values θ k and θ l can yield equal optimal discrete solution values, i.e., for one or more time steps i e {0, 1,..., N} in the prediction horizon, In this case, the corresponding class labels are equal to each other Since the class label values are unique identifiers of the discrete optimal solution values in the discrete optimizer set Δ.

[0229] Some embodiments of the present application are based on the recognition that by exploiting the temporal structure of the discrete MIOCP solution, the number of target class labels in supervised learning can be significantly reduced, which can result in increased sampling efficiency and / or improved performance of the machine learning based predictor. For example, given n δ discrete variables at each time step i = 0, 1,..., N, the maximum number of target class label values is By exploiting the temporal structure of the MIOCP, this can be reduced to at most target class label values at each time step. Furthermore, some embodiments of the present application are based on the recognition that due to the sampling of problem parameter values that represent optimal control problems of interest in practice and due to the constraints on the feasibility of the MIOCP problem, the number of unique discrete values can be much smaller than the theoretical maximum number of target class label values.

[0230] In some embodiments of the present application, the supervised learning based predictor is trained based on a regression problem, i.e., the predictor is trained to directly predict the optimal values of the discrete variables at each time step i = 0, 1,..., N in the control horizon of the MIOCP. In some embodiments of the present application, the predictor is an RNN, e.g., consisting of one or more LSTMs and / or one or more feedforward neural networks, with input dimensions equal to the number of problem parameters n θ and output dimensions equal to the number of discrete variables at each time step n δ . For example, the training loss of the regression problem can be binary cross-entropy loss, with a logit activation applied to the output layer of the network. In some embodiments of the present application, the regressor is used to fix only one or more of the discrete variables for which the learning based predictor has a relatively high confidence, such that the remaining free discrete variables need to be solved for a reduced MIOCP to compute a feasible but possibly suboptimal mixed integer solution of the high-dimensional MIOCP problem.

[0231] In some embodiments of the present invention, a supervised learning based predictor is trained based on a multi-class classification problem, i.e., the predictor is trained to select the optimal discrete value from a set of discrete optimizers, i.e. In some embodiments of the present invention, the predictor is an RNN, for example composed of one or more LSTMs and / or one or more feed-forward neural networks, with an input dimension equal to the number of problem parameters n θ , and the output dimension is equal to the number of unique discrete values ​​|Δ| in the dataset. For example, the training loss for the classification problem can be a cross-entropy loss. In some embodiments of the present invention, a classifier is used to select one or more discrete solution candidates corresponding to the highest confidence of feasibility and optimality, and an iterative pre-solve-based correction method can be used to potentially correct one or more of these discrete solution candidates, thereby computing a feasible but potentially suboptimal mixed integer solution to the high-dimensional MIOCP problem.

[0232] FIG. 10C 1030 and an iterative pre-solve-based correction step 1035, and an optimal control structured CP solution process 1040 for calculating a feasible MICP solution vector 635 according to some embodiments of the present invention. In some embodiments of the present invention, given the problem parameter values ​​θ, the discrete optimizer set Δ, the trained machine learning network H φ and the maximum number of evaluations n evals In the case of 1060. When the online variable fixation and optimal control solution process 1020 begins, the initialization step 1061 sets k←1, and the optimal control solution z ＊ =(x ＊ ,u ＊ ) and δ ＊ Each is set equal to the empty set, and the optimal objective value is set to infinity J ＊ ←∞.

[0233] In some embodiments of the present invention, online variable fixing 1020 is an iterative process, and each iteration is performed by guessing the discrete solution. Iterative pre-solved correction steps are used to calculate the new discrete solution value 1062. If the correction is unsuccessful 1063, for example, because the discrete solution guessed using the prediction If no feasible solution is found, the process continues with the next iteration and the number of iterations may be updated to k←k+1 1068. If the correction successfully computes a new value with an increased likelihood of feasibility and / or optimality 1062, then the online variable fixing procedure 1020 proceeds by solving the optimal control structured CP for the fixed discrete value to compute a feasible and optimal control solution and the corresponding objective value 1064.

[0234] In case there is no feasible solution of the optimal control structured CP for the fixed discrete value or if the corresponding objective value is greater than the current optimal objective value 1065, then the procedure continues with the next iteration and the iteration number can be updated as k <- k + 1 1068. If there is a feasible solution of the optimal control structured CP for the fixed discrete value and if the corresponding objective value is smaller than the current optimal objective value 1065, then the procedure updates the optimal control solution its updated optimal discrete solution and the optimal objective value 1066. In some embodiments of the application, in order to limit the total computation time of the online variable fixing and optimal control solution, a maximum number of evaluations n evals is defined, such that if the maximum number k + 1 > n evals 1067, then the online variable fixing procedure 1020 is terminated.

[0235] If the maximum number of evaluations has not been reached 1067, then the procedure continues with the next iteration and the iteration number can be updated as k <- k + 1 1068. If the maximum number of evaluations has been reached 1067, then the optimal control solution vector (z ＊ , δ ＊ , J ＊ ) 635 can be used to implement a predictive controller according to embodiments of the application.

[0236] In some embodiments of the application, the iterative pre-solution based correction method 1062 can return any of the following results:

[0237] ■The iterative correction method detects that the predicted discrete value set is feasible, such that the value is returned;

[0238] ■The iterative correction method computes an updated set of feasible discrete values such that the value is returned;

[0239] ■The iterative correction method can not be able to compute an updated set of feasible discrete values within a predetermined time limit.

[0240] Embodiments of the present application are based on the recognition that an iterative pre-solved based correction method can increase the likelihood of feasibility and / or optimality, e.g., when the iterative correction method removes or prunes discrete variables for which a machine learning based predictor provides incorrect predicted values that lead to infeasible control solutions.

[0241] The following description provides example embodiments only, and is not intended to limit the scope, applicability or configuration of the disclosure. Rather, the following description of the example embodiments will provide enabling descriptions for one of ordinary skill in the art to implement one or more example embodiments. Various changes can 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.

[0242] In the following description, specific details are set forth to provide a thorough understanding of the embodiments. However, persons having ordinary skill in the art will appreciate that the embodiments can be practiced without the specific details. For example, systems, processes and other elements in the disclosed subject matter can be shown as components in block diagram form, rather than implementing specific hardware and firmware in order not to obscure the embodiments in unnecessary detail. In other instances, well-known processes, structures and techniques have not been shown in detail in order not to obscure the embodiments.

[0243] Furthermore, various embodiments can be described as a process depicted as a flowchart, flow diagram, data flow diagram, structure diagram, or block diagram. Although a flowchart can describe operations as a sequential process, many of the operations can be performed in parallel or concurrently. In addition, the order of the operations can be re-arranged. A process can be terminated when its operations are completed, but could have additional steps not discussed or included in a figure. A process can correspond to a method, a function, a procedure, a subroutine, a subprogram, etc. When a process corresponds to a function, its termination can correspond to a return of the function to the calling function or the main function.

[0244] Furthermore, embodiments of the disclosed subject matter can be implemented, at least in part, manually or automatically. Manual or automatic implementation can occur contemporaneously with the execution of software, firmware, middleware, microcode, hardware description languages, or any combination thereof. A program code or code segment that is used to implement a process, e.g., a software program, can be stored in a machine-readable medium. A processor can execute the necessary tasks.

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

[0246] The above-described embodiments of the application can be implemented in any of various ways. For example, the embodiments can be implemented using hardware, software or a combination thereof. When implemented in software, the software code can be executed on any suitable processor or collection of processors, whether provided in a single computer or distributed among multiple computers. Such processors can be implemented as integrated circuits, with one or more processors in an integrated circuit component. However, a processor can be implemented using circuitry in any suitable format.

[0247] Embodiments of the disclosure can be implemented as a method, of which an example has been provided. The acts performed as part of the method can be ordered in any suitable way. Accordingly, embodiments can be constructed in which acts are performed in an order different than illustrated, which can include performing some acts simultaneously, even though shown as serial act in illustrative embodiments.

[0248] While the present disclosure has been described with reference to certain preferred embodiments thereof, a worker skilled in the art will recognize that changes can be made in form and detail without departing from the spirit and scope of the present disclosure. Accordingly, the described aspects of the claims are to be understood as covering all such variations and modifications as fall within the true spirit and scope of the present disclosure.

Claims

1. A controller for controlling a vehicle traveling on a road having a geometric design defined by one or a combination of a line of tangency, a section, and a cross-section of the road, wherein: Different values of parameters of the geometry design of the road, traffic on the road, traffic rules for traffic flow on the road define different traffic scenarios, the controller comprising: at least one processor; and a memory having instructions stored thereon that, when executed by the at least one processor, cause the controller to: collect parameters for controlling the vehicle for a current real-world traffic scenario, the parameters including configuration parameters that cause non-convexity of a mixed-integer non-convex constrained optimization problem for simultaneous decision and motion planning for the vehicle and limiting parameters that are independent of the non-convexity of the mixed-integer non-convex constrained optimization problem; convert the mixed-integer non-convex constrained optimization problem for the current real-world traffic scenario into a mixed-integer convex optimization problem for an approximate representation of the real-world traffic scenario by relaxing the configuration parameters and tightening corresponding limiting parameters; solve the converted mixed-integer convex optimization problem for the approximate representation of the real-world traffic scenario to produce current control commands for controlling one or more actuators of the vehicle; and control the one or more actuators of the vehicle according to the control commands.

2. The controller of claim 1, wherein, the memory stores predetermined traffic scenarios that cause the mixed-integer convex optimization problem, wherein the controller converts the parameters of a current traffic scenario to the parameters of the predetermined traffic scenarios.

3. The controller of claim 1, wherein, tightening the limiting parameters causes one or more updated boundary constraints in the mixed-integer convex optimization problem, e.g., updated limits on steering and / or lateral velocity of a controlled vehicle to account for curvature of one or more road segments along a route from a current location of the controlled vehicle to a desired destination in a traffic network.

4. The controller of claim 2, wherein, the predetermined traffic scenarios include one or a combination of: a controlled vehicle on a straight road segment having one or more lanes, predicted motion trajectories of one or more other vehicles and / or traffic participants, one or more obstacle avoidance constraints for the controlled vehicle to remain outside a safety region around the predicted motion trajectories of the one or more other vehicles and / or traffic participants, lane change timing constraints, one or more conflict region constraints for individual traffic intersections and / or merging points, and one or more spatially dependent region constraints along a future planned route of the controlled vehicle in a traffic network.

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

6. The controller of claim 4, wherein, the predicted motion trajectories of the one or more other vehicles and / or traffic participants are computed using closed-loop kinematic vehicle models for lane keeping and / or adaptive cruise control, taking into account that individual ones of the other vehicles and / or traffic participants remain in a current lane during a prediction time horizon of a vehicle decision and motion planning system for the controlled vehicle.

7. The controller of claim 4, wherein, The predicted motion trajectory of the one or more other vehicles and / or traffic participants is computed using a data-based vehicle model that aims to predict the behavior of human drivers during a prediction time span for a vehicle decision and motion planning system of the controlled vehicle, wherein the data-based vehicle model uses one or a combination of deep learning, support vector machines, neural networks, neural processes, Gaussian processes, machine learning techniques, and stochastic prediction models.

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

9. The controller of claim 8, wherein, The approximate solution of the mixed-integer non-convex optimization problem comprises a reference motion trajectory used by a vehicle tracking control system such that the controlled vehicle achieves an actual motion trajectory that is as close as possible to the reference motion trajectory provided by the decision and motion planning system; wherein the vehicle tracking control system uses a linear, linear time-varying, or nonlinear model predictive controller, MPC, to achieve.

10. 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 quadratic constraint quadratic programming, MIQCQP, problem, or a mixed-integer second order cone programming, MISOCP, problem.

11. The controller of claim 1, wherein, The global optimal solution or a feasible but suboptimal solution of the mixed-integer convex programming, MICP, problem can be computed at each control time step, e.g., using a branch-and-bound method, a branch-and-cut method, a branch-and-price method, or any other variant of a tree search based optimization algorithm.

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

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

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

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

16. The controller of claim 12, wherein, The deep learning network architecture comprises one or a combination of a deterministic model and a stochastic prediction model; wherein the deterministic model comprises one or a combination of a multilayer perceptron, a convolutional neural network (CNN), a recurrent neural network (RNN), a transformer, a kernel regression, and a support vector machine, and wherein the stochastic model comprises one or a combination of a Bayesian neural network, a neural process, a Gaussian process, and a Kriging interpolation.

17. A method for controlling a vehicle traveling on a road having a geometric design defined by one or a combination of a tangent, a break, and a cross section of the road, wherein, Different values of parameters of a geometric design of a road, traffic on the road, traffic rules for a flow of traffic on the road define different traffic scenarios, wherein the method uses a processor coupled to a memory having instructions stored thereon that, when executed by the processor, perform the steps of the method, the instructions comprising: collecting parameters for controlling the vehicle for a current real-world traffic scenario, the parameters including configuration parameters that cause non-convexity of a mixed-integer non-convex constrained optimization problem for simultaneous decision and motion planning for the vehicle and limit parameters that are independent of the non-convexity of the mixed-integer non-convex constrained optimization problem; transforming the mixed-integer non-convex constrained optimization problem for the current real-world traffic scenario into a mixed-integer convex optimization problem for an approximate representation of the real-world traffic scenario by relaxing the configuration parameters and tightening corresponding limit parameters; solving the transformed mixed-integer convex optimization problem for the approximate representation of the real-world traffic scenario to produce 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.

18. The method of claim 17, wherein, The memory stores predetermined traffic scenarios that result in the mixed-integer convex optimization problem, wherein the method transforms the parameters of a current traffic scenario to the parameters of the predetermined traffic scenarios.

19. The controller of claim 17, wherein, Tightening 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 velocity of a controlled vehicle to account for curvatures of one or more road segments in a traffic network along a route from a current location of the controlled vehicle to a desired destination.

20. 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 having a geometric design defined by one or a combination of alignments, profiles, and cross-sections of the road, wherein, Different values of parameters of a geometric design of a road, traffic on the road, traffic rules for a flow of traffic on the road define different traffic scenarios, the method comprising: collecting parameters for controlling the vehicle for a current real-world traffic scenario, the parameters including configuration parameters that cause non-convexity of a mixed-integer non-convex constrained optimization problem for simultaneous decision and motion planning for the vehicle and limit parameters that are independent of the non-convexity of the mixed-integer non-convex constrained optimization problem; transforming the mixed-integer non-convex constrained optimization problem for the current real-world traffic scenario into a mixed-integer convex optimization problem for an approximate representation of the real-world traffic scenario by relaxing the configuration parameters and tightening corresponding limit parameters; solving the transformed mixed-integer convex optimization problem for the approximate representation of the real-world traffic scenario to produce 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. controlling the one or more actuators of the vehicle in accordance with the control commands.

Images