Adaptive control of autonomous or semi-autonomous vehicles

Abstract

A control system uses a probabilistic motion planner and an adaptive predictive controller to control a vehicle. The probabilistic motion planner generates a parameterized probability distribution sequence for a target state sequence for the vehicle using parameters defining first-order moments and higher-order moments. The adaptive predictive controller optimizes a cost function within a prediction horizon to generate a sequence of control commands for one or more actuators of the vehicle. The cost function balances the tracking costs of different state variables in the target state sequence defined by the first-order moments. This balancing is performed by weighting the different state variables using one or more higher-order moments of the probability distribution.

Description

Technical Field

[0001] The present invention relates generally to controlling vehicles and, more particularly, to controlling autonomous or semi-autonomous vehicles. Background Art

[0002] Autonomous vehicles are complex decision-making systems that require the integration of advanced and interconnected sensing and control components. At the highest level, a route planner calculates a sequence of destinations through a road network. Discrete decision layers are responsible for determining the vehicle's local driving goals. Individual decisions can be to turn right, stay in a lane, turn left, or come to a complete stop in a specific lane at an intersection. The sensing and mapping module uses various sensor information (such as radar, laser radar (LIDAR), cameras, and global positioning system (GPS) information) together with previous map information to estimate the portion of the surrounding environment that is relevant to the driving scenario.

[0003] The output of motion planning is the input to the vehicle controller. The motion planner is responsible for determining a safe, desirable, and dynamically feasible trajectory for the vehicle to follow based on the output from the sensing and mapping modules. The vehicle control algorithm then aims to track this reference motion by issuing commands (e.g., steering angle, wheel torque, and braking force) at a relatively high sampling frequency. Finally, the actuator control layer adjusts the actuators to implement these requested commands.

[0004] The motion planning problem in autonomous vehicles shares many similarities with the standard robotics setting, and due to the non-convexity of the problem, the optimal solution is in most cases very difficult. Methods that rely on direct dynamic optimization have been developed for specialized scenarios. However, due to the non-convexity, this only leads to local optimal solutions, which may be significantly far from the global optimal solution and may come at the cost of relatively large computational load and time. Motion planning is usually performed using sampling-based methods such as rapidly exploring random trees (RRT) or graph search methods such as A*, D* and other variants.

[0005] Some methods perform sampling deterministically, while other methods (such as the method described in U.S. Patent 9,568,915) use probabilistic sampling. Sampling techniques are suitable for fast machine calculations, but the paths generated by sampling methods may not feel natural to passengers in autonomous or semi-autonomous vehicles. Therefore, there is still a need for improved path planning and control of autonomous or semi-autonomous vehicles. Summary of the Invention

[0006] Some embodiments disclose an integrated architecture for a (semi-)autonomous driving system, involving a low-rate, long-term sampling-based motion planning algorithm and a high-rate, highly reactive optimization-based predictive vehicle controller. This integration enables systematic and efficient sharing of the burden of achieving multiple competing objectives (e.g., ensuring safety requirements are met and ensuring reliable and comfortable driving behavior in relatively complex highway and urban driving scenarios) across the motion planning and vehicle control layers.

[0007] The motion planner computes the first-order moments of a reference trajectory to be tracked, and it computes higher-order moments that define corresponding confidence bounds on the planned trajectory. Since our planning algorithm generates the target trajectory and its confidence from, for example, a covariance matrix, instead of just using this covariance matrix as the target to be tracked by the controller, we also use this controller here, specifically for tuning the time-varying tracking cost function in the optimal control problem formulation of the predictive controller. This results in an automatic tuning mechanism for trading off between multiple competing objectives. In effect, the confidence associated with a trajectory indicates to what extent the path planner believes that its calculated trajectory is valid. By using the confidence information to automatically tune the cost function of the predictive controller, we allow more deviation when such confidence is low and less deviation when such confidence is relatively high.

[0008] Some embodiments of the present invention are based on a probabilistic approach to motion planning that uses particle filtering to approximate the involved probability density functions (PDFs). Since particle filter-based motion planners compute the PDF of the state trajectory, higher-order moments than the weighted average can be determined. For example, by computing the covariance matrix (i.e., second-order moments) along the weighted average, we can determine a Gaussian approximation of the desired reference trajectory, which is directly used to formulate and adapt the tracking cost function in the predictive controller.

[0009] Some embodiments of the present invention utilize an implementation of the vehicle control layer based on an adaptive implementation of a linear or nonlinear model predictive control (MPC) with a time-varying formulation of a staged least squares cost function. Specifically, the MPC cost function can be defined using a time-varying tracking weight matrix. Because the path planner provides both first-order and higher-order moments of the planned motion trajectory, this information can be directly used to tune both the reference values ​​and the weighting matrix in the staged least squares tracking cost function.

[0010] Some embodiments are based on an inversely proportional relationship between the uncertainty of the motion planner and the tracking cost in the MPC problem, resulting in a time-varying sequence of weight matrices. More specifically, the uncertainty of the reference motion plan is expected to increase as the vehicle is predicted to move relatively close to the vehicle's safety-related constraints, so that the penalty for deviating from the reference trajectory will decrease, and vice versa. This in turn allows the MPC to predict larger or smaller deviations of the state and / or control trajectory from its reference (i.e., the motion plan trajectory), respectively.

[0011] For autonomous vehicles, using simple moments such as variance is insufficient for safe vehicle operation. For example, consider a situation where there is a vehicle in front of the autonomous vehicle, and it is possible to stop behind it or overtake it by changing lanes. If the motion planner has already determined the distribution of possible trajectories, determining the variance of this distribution will be overly conservative and lead to poor performance.

[0012] Therefore, one embodiment addresses this conservatism by determining different patterns from the distribution. For example, one pattern may be the distribution of possible trajectories corresponding to lane keeping, while another pattern may be the distribution of possible trajectories corresponding to lane changing. Based on these different patterns, the variance of each individual pattern can be calculated, thereby reducing conservatism.

[0013] Some embodiments are based on the recognition that the motion planner can exploit information about the specific current state of the vehicle control algorithm. For example, MPC is based on a constrained optimization method that includes obstacle avoidance inequality constraints. If the variance propagated from the motion planner to the MPC is relatively small, the MPC controller may unnecessarily activate obstacle avoidance constraints, resulting in an unsteady trajectory. To this end, in one embodiment of the present invention, the MPC informs the motion planner about the latest amount of constraint activations and / or constraint violations in the MPC's predicted state and control trajectory, which can be used to adjust the confidence, i.e., increase or decrease the variance of the trajectory distribution in the motion planner. This results in increased safety and improved smoothness of the planned reference trajectory.

[0014] Different implementations use different time scales for real-time operation of the motion planning and vehicle control layers. For example, in one implementation, the motion planner computes a long-term, highly predictive reference trajectory, but it typically needs to run at a relatively low sampling frequency, i.e., it has a relatively slow update rate (e.g., every 1 second) and is therefore quite inactive. In contrast, MPC typically uses a much shorter prediction horizon, but it runs at a much higher sampling frequency (e.g., every 25 milliseconds), allowing the controller to be highly reactive to local deviations due, for example, to uncertainty in the pose estimate of the ego vehicle and surrounding obstacles. Therefore, sharing the responsibility between the planning and control layers to ensure safe and reliable driving behavior is important, especially in order to meet real-time and safety requirements under uncertainty.

[0015] Thus, one embodiment discloses a system for controlling a vehicle, the system comprising: an input interface configured to accept a current state of the vehicle, an image of an environment proximate to the current state of the vehicle, and a destination of the vehicle; a memory configured to store a probabilistic motion planner and an adaptive predictive controller, wherein the probabilistic motion planner is configured to accept the current state of the vehicle, the destination of the vehicle, and the image of the environment to generate a sequence of parameter probability distributions about a sequence of target states defining a motion plan for the vehicle, wherein a parameter of each parameter probability distribution defines a first order moment of the probability distribution. moment) and at least one higher-order moment, wherein the adaptive predictive controller is configured to optimize a cost function within a prediction range to generate a sequence of control commands for one or more actuators of the vehicle, wherein the optimization of the cost function balances the tracking costs of different state variables in a target state sequence defined by a first-order moment, wherein in the balancing of the tracking costs, one or more of the higher-order moments of the probability distribution are used to weight the different state variables; a processor configured to execute a probabilistic motion planner by submitting a current state of the vehicle, a destination of the vehicle, and an image of the environment to the probabilistic motion planner, and the processor is configured to execute the adaptive predictive controller by submitting a parameter probability distribution sequence generated by the probabilistic motion planner to the adaptive predictive controller to generate a sequence of control commands; and an output interface configured to output at least one control command determined by the adaptive predictive controller to at least one actuator of the vehicle.

[0016] Another embodiment discloses a method for controlling a vehicle, wherein the method uses a processor coupled to a memory storing a probabilistic motion planner and an adaptive predictive controller, wherein the probabilistic motion planner is configured to accept a current state of the vehicle, a destination of the vehicle, and an image of the environment to generate a sequence of parametric probability distributions about a sequence of target states defining a motion plan for the vehicle, wherein parameters of each parametric probability distribution define a first-order moment and at least one higher-order moment of the probability distribution, wherein the adaptive predictive controller is configured to optimize a cost function within a prediction horizon to generate a sequence of control commands for one or more actuators of the vehicle, wherein the optimization of the cost function balances tracking costs of different state variables in the sequence of target states defined by the first-order moments, wherein in tracking In balancing costs, one or more high-order moments of the probability distribution are used to weight different state variables, wherein a processor is connected to stored instructions for executing the method, wherein the instructions, when executed by the processor, perform the steps of the method, and the method includes the following steps: accepting a current state of the vehicle, an image of the environment close to the current state of the vehicle, and a destination of the vehicle; executing a probabilistic motion planner by submitting the current state of the vehicle, the destination of the vehicle, and the image of the environment to the probabilistic motion planner; executing an adaptive predictive controller by submitting a parameter probability distribution sequence generated by the probabilistic motion planner to the adaptive predictive controller to generate a control command sequence; and submitting at least one control command determined by the adaptive predictive controller to at least one actuator of the vehicle.

[0017] Yet another embodiment discloses a non-transitory computer-readable storage medium having embodied thereon a program executable by a processor to perform a method, wherein the medium stores a probabilistic motion planner and an adaptive predictive controller, wherein the probabilistic motion planner is configured to accept a current state of a vehicle, a destination of the vehicle, and an image of an environment to generate a sequence of parameter probability distributions regarding a sequence of target states defining a motion plan for the vehicle, wherein parameters of each parameter probability distribution define a first-order moment and at least one higher-order moment of the probability distribution, wherein the adaptive predictive controller is configured to optimize a cost function within a prediction range to generate a sequence of control commands for one or more actuators of the vehicle, wherein the optimization of the cost function balances tracking costs of different state variables in the sequence of target states defined by the first-order moments, and wherein in the balancing of the tracking costs, one or more of the higher-order moments of the probability distribution are used to weight the different state variables.

[0018] The method includes the following steps: accepting a current state of a vehicle, an image of an environment proximate to the current state of the vehicle, and a destination of the vehicle; executing a probabilistic motion planner by submitting the current state of the vehicle, the destination of the vehicle, and the image of the environment to the probabilistic motion planner; executing an adaptive predictive controller by submitting a parameter probability distribution sequence generated by the probabilistic motion planner to the adaptive predictive controller to generate a control command sequence; and submitting at least one control command determined by the adaptive predictive controller to at least one actuator of the vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 is a block diagram of a predictive controller and feedback system according to some embodiments.

[0020] Figure 2A is a schematic diagram of a vehicle including a controller employing the principles of some embodiments; and

[0021] Figure 2B is a schematic diagram of the interaction between a controller employing the principles of some embodiments and a controller of a vehicle 201 according to some embodiments.

[0022] Figure 3A A schematic diagram showing a multi-layer control structure including a decision maker, a motion planner, a vehicle controller, and an actuator controller, and the information exchange between these layers.

[0023] Figure 3B A schematic diagram of a multi-layer control structure including a decision maker, a probabilistic motion planner, a model predictive controller, and an actuator controller and the information exchange between these layers is shown.

[0024] Figure 3C Automatic tuning of one or more cost functions of a model predictive controller in a multi-layer control structure based on command information from a probabilistic motion planner is illustrated.

[0025] Figure 4A An exemplary list of possible control objectives for vehicle behavior driving on a road and their corresponding control functions in a motion planner is illustrated according to some embodiments.

[0026] Figure 4B A schematic diagram illustrating the structure of a probabilistic control function employed by a motion planner in some embodiments.

[0027] Figure 4C An example of the impact of the probabilistic output of a motion planner on the performance of a vehicle controller is shown in accordance with some embodiments.

[0028] Figure 4DAn example of the impact of the probabilistic output of a motion planner on the performance of a vehicle controller is shown in accordance with some embodiments.

[0029] Figure 4E An illustration of a method of balancing the importance of different control functions according to some embodiments is shown.

[0030] Figure 5A A block diagram of an adaptive model predictive control (MPC) method that solves an inequality-constrained optimization problem at various time steps to implement a vehicle controller is shown, according to some embodiments.

[0031] Figure 5B A block diagram of an adaptive model predictive control (MPC) method that solves an inequality-constrained optimization problem at various time steps to implement a vehicle controller is shown, according to some embodiments.

[0032] Figure 6A is a block diagram of an adaptive MPC method for solving an optimal control structured quadratic program (QP) according to some embodiments.

[0033] Figure 6B is a block diagram of an adaptive MPC method for solving an optimal control structured nonlinear program (NLP) according to some embodiments.

[0034] Figure 7A is a block diagram of a method of adapting reference and weight matrices for a particular implementation of a time-varying reference tracking cost in an adaptive linear or nonlinear MPC controller.

[0035] Figure 7B is a block diagram of a method of adapting reference and diagonal weight matrices for a particular implementation of a time-varying reference tracking cost in an adaptive linear or nonlinear MPC controller.

[0036] Figure 7C This paper describes an algorithm for automatically tuning the time-varying reference tracking cost of an adaptive MPC controller using a sequence of reference values ​​and a covariance matrix from a probabilistic motion planner.

[0037] Figure 8 A schematic diagram illustrating a state transition tree defining vehicle motion according to some embodiments of the present invention is shown.

[0038] Figure 9A A flow chart of a probabilistic motion planner for determining a sequence and probability distribution of vehicle states according to some embodiments of the present invention is shown.

[0039] Figure 9B A flow chart illustrating a method of determining the probability that each state is consistent with a probabilistic control function.

[0040] Figure 9C An example is shown where the prediction of the ego vehicle's position intersects an uncertainty region for an obstacle and where the probability of the obstacle being at that position is above a certain collision threshold.

[0041] Figure 9D A simplified schematic diagram showing the results of three iterations of the steps for generating states consistent with the probabilistic control function.

[0042] Figure 9E Shown in Figure 9D The probabilities of the five possible assignments to the states for the first iteration in .

[0043] Figure 10A An example diagram of a control function corresponding to the control objective of keeping the vehicle on the road is shown.

[0044] Figure 10B An illustrative diagram showing a control function corresponding to a control target of shortening a vehicle travel time on a road.

[0045] Figure 10C An example diagram of a control function for expressing the driving goal of safely passing an obstacle is shown.

[0046] Figure 10D An example diagram is shown of a possible way to describe a control function that targets maintaining a current speed according to some embodiments. DETAILED DESCRIPTION

[0047] Some embodiments of the present invention provide systems and methods for controlling the operation of a system or system using a predictive controller.An example of a predictive controller is model predictive control (MPC), which determines control inputs based on a model of the controlled system.

[0048] Figure 1 An example system 120 is shown connected to a predictive controller 110 via a state estimator 130, according to some embodiments. In some implementations, the predictive controller is an MPC controller programmed according to a dynamic model 102 of the system. The model can be a set of equations representing how the state and output 103 of the system 120 change over time based on current and previous inputs 111 and previous outputs 103. The model can include constraints 104 representing the physical and operational limitations of the system. During operation, the controller receives a command 101 indicating a desired behavior for the system. The command can be, for example, a motion command. In response to receiving the command 101, the controller generates a control signal 111 that serves as input to the system. In response to the input, the system updates the output 103 of the system. Based on measurements of the output of the system 103, the estimator updates an estimated state 121 of the system. This estimated state 121 of the system provides state feedback to the controller 110.

[0049] As referred to herein, a system 120 can be any machine or device that is controlled by some manipulated input signal 111 (input), which may be associated with a physical quantity such as voltage, pressure, force, torque, and returns some controlled output signal 103 (output), which may be associated with a physical quantity such as current, flow, velocity, position, that indicates a transition of the state of the system from a previous state to a current state. The output value is partially related to the previous output value of the system and partially related to the previous input value and the current input value. The dependence on the previous input and the previous output is encoded in the state of the system. The operation of the system (e.g., the movement of a component of the system) can include a sequence of output values ​​generated by the system after applying certain input values.

[0050] The model 102 of a system may include a set of mathematical equations that describe how the system's output changes over time as a function of current and previous inputs and previous outputs. The state of a system is any set of information that generally changes over time, such as a proper subset of current and previous inputs and outputs, that, together with the model of the system and future inputs, can uniquely define the future behavior of the system.

[0051] The system may be subject to physical limitations and regulatory constraints 104 that limit the range in which the system's outputs, inputs, and possible states are allowed to operate.

[0052] The controller 110 can be implemented in hardware or as a software program executed in a processor (e.g., a microprocessor), which receives the estimated state 121 of the system and the desired motion command 101 at a fixed or variable control period sampling interval and uses this information to determine input for operating the system (e.g., control signal 111).

[0053] The command 101 may include a reference command 105 and a representation of a confidence level 106 for the reference command 105. For example, the command 101 may include a reference motion 105 and confidence limits 106 around the reference motion. In some embodiments of the invention, the reference motion 105 may be represented by a reference trajectory of state and / or output values, and the confidence limits may be represented by a covariance matrix defining the uncertainty around the reference trajectory of the state and / or output values. In some embodiments of the invention, the command 101 is calculated by a probabilistic motion planner, and the reference motion 105 corresponds to a first-order moment of the statistics of the motion plan, and the confidence levels 106 correspond to second-order or higher-order moments of the statistics of the motion plan.

[0054] The estimator 130, which may be implemented in hardware or as a software program executed in the same or a different processor than the controller 110, receives the output 103 of the system at a fixed or variable control period sampling interval and uses new and previous output measurements to determine an estimated state 121 of the system 120.

[0055] Figure 2A A schematic diagram of a vehicle 201 including a predictive controller 202 employing the principles of some embodiments is shown. As used herein, vehicle 201 can be any type of wheeled vehicle, such as a passenger car, bus, or rover. Furthermore, vehicle 201 can be an autonomous or semi-autonomous vehicle. For example, some embodiments control the motion of vehicle 201. Examples of motion include lateral motion of the vehicle controlled by a steering system 203 of vehicle 201. In one embodiment, steering system 203 is controlled by controller 202. Additionally or alternatively, steering system 203 can be controlled by a driver of vehicle 201.

[0056] The vehicle may also include an engine 206, which may be controlled by the controller 202 or other components of the vehicle 201. The vehicle may also include one or more sensors 204 for sensing the surrounding environment. Examples of sensors 204 include rangefinders, radars, lidars, and cameras. The vehicle 201 may also include one or more sensors 205 for sensing its current motion and internal state. Examples of sensors 205 include: global positioning systems (GPS), accelerometers, inertial measurement units, gyroscopes, shaft rotation sensors, torque sensors, deflection sensors, pressure sensors, and flow sensors. These sensors provide information to the controller 202. The vehicle may be equipped with a transceiver 207, which is capable of communicating with the controller 202 via a wired or wireless communication channel.

[0057] Figure 2B A schematic diagram illustrates the interaction between predictive controller 202 and controller 220 of vehicle 201, according to some embodiments. For example, in some embodiments, controller 220 of vehicle 201 may include a steering controller 225 and a brake / throttle controller 230, which control the rotation and acceleration of vehicle 220. In such a case, predictive controller 202 outputs control inputs to controllers 225 and 230 to control the state of the vehicle. Controller 220 may also include a higher-level controller, such as a lane keeping assist controller 235, which further processes the control inputs from predictive controller 202. In both cases, controller 220 uses the outputs of predictive controller 202 to control at least one actuator of the vehicle, such as the vehicle's steering wheel and / or brakes, to control the vehicle's motion.

[0058] Figure 3A A schematic diagram of a multi-layered control and decision-making architecture for an autonomous or semi-autonomous vehicle is shown. Autonomous vehicles are complex systems that require the integration of advanced and interconnected sensing and control components. Embodiments of the present invention include a motion planning layer 310 and a vehicle controller 320. The motion planner 310 calculates motion commands 101 and provides these motion commands to the vehicle controller 320. Based on the references 105 and confidence levels 106 in the motion commands 101, the vehicle controller 320 calculates control inputs 111 for the system to execute the motion commands. Some embodiments of the present invention also include a decision-making layer 300 and / or an actuator controller 330.

[0059] At the highest level, a sequence of destinations may be calculated by a route planner through a road network. Given a route, a discrete decision layer 300 may be responsible for determining one or more local driving goals for the vehicle and corresponding discrete decisions 301. Individual decisions may be any of turning right, staying in a lane, turning left, or coming to a complete stop in a particular lane at an intersection. The decision layer 300 uses information from one or more sensors (such as radar, lidar, inertial measurement unit, camera, and / or global positioning system (GPS) information) along with prior map information to estimate the state of the system and the portion of the surrounding environment that is relevant to the system for a particular driving scenario. The estimated information from the sensing and mapping modules can be used to Figure 3A One, more or all of the control and decision-making layers of the structure.

[0060] Based on one or more local destination goals 301, the motion planner 310 is responsible for determining motion commands 101 that are provided to the vehicle controller 320. In some embodiments, the motion commands include a reference command 105 and a confidence bound 106. In some embodiments, the reference command is a safe, desired, and dynamically feasible trajectory that the vehicle should follow based on the output from the decision layer 300. Some embodiments of the present invention are based on the recognition that an important requirement is that the reference trajectory 105 calculated by the motion planner 310 is collision-free, dynamically feasible, and can be tracked by the vehicle controller 320. This means that the reference trajectory achieves one or more local driving goals while avoiding any collisions with the environment and obeying the dynamic model 102 of the system, which can be represented by a set of mathematical equations.

[0061] Some embodiments of the present invention are based on such recognition that the typical limiting factor in motion planning tasks is the non-convex nature of the corresponding constrained dynamic optimization problem. This results in only achieving local optimal solutions that may be significantly away from the global optimal solution, and may require very large computational loads and time to even only find feasible solutions. For example, motion planning is performed using sampling-based methods (such as rapidly exploring random trees (RRT)) or graph search methods (such as A*, D* and other variations).

[0062] like Figure 3B As shown, some embodiments of the present invention use a probabilistic approach for motion planning 311, for example, using a particle filter to approximate the involved probability density functions (PDFs). Driving requirements (such as staying on the road, left-hand or right-hand traffic, and obstacle avoidance) can be formulated as measures of a nonlinear filtering problem. The resulting tree expansion can differ from the standard RRT algorithm because the particle filter-based probabilistic motion planner 311 does not sample the state space, but instead samples the input space and adds additional correction terms based on the driving requirements.

[0063] In some embodiments of the invention, the probabilistic motion planner 311 accepts the current state of the vehicle, the destination of the vehicle, and an image of the environment to generate a sequence of parametric probability distributions about a sequence of target states and / or output values ​​that define motion commands for the vehicle, wherein the parameters of each parametric probability distribution define a first-order moment 316 and at least one higher-order moment 317 of the probability distribution. In some embodiments of the invention, a cost function may be minimized by the probabilistic motion planner 311 such that a desired behavior is achieved in the motion commands for the vehicle.

[0064] Some embodiments of the present invention are based on the recognition that each target state of the vehicle may include multiple state variables, so that each parameter probability distribution in the probabilistic motion planner 311 is a multivariate distribution of parameters defining the distribution of each state variable, in which case the first-order moment of each parameter probability distribution is the mean and the higher-order moments of the parameter probability distribution are covariance matrices with values ​​that vary in space and time.

[0065] In some embodiments of the present invention, the probabilistic motion planner 311 includes a particle filter-based algorithm having a set of particles propagated in time to represent a set of likelihoods of the target state at a time instance, such that each particle comprises a Gaussian distribution of the value of the target state at that time instance. In this case, the first-order moment of the parameter probability distribution at the time instance is the weighted average of the particles, and the higher-order moments of the parameter probability distribution at the time instance are the weighted covariances of the particles.

[0066] like Figure 3A and Figure 3B As shown, the vehicle controller 320 and / or 340 aims to achieve the reference motion 105 by calculating a control signal 321 for operating the system, taking into account the corresponding confidence limits 106. The control signal may include one or more actuation commands, such as, for example, values ​​for steering angle, wheel torque, and braking force. In some embodiments of the present invention, the vehicle controller 320 provides the control signal 321 to an additional layer consisting of one or more controllers 330, which directly adjust the actuators to achieve the requested vehicle behavior.

[0067] Different embodiments of the present invention may use different techniques in the vehicle controller 320 to track the reference motion 105 calculated by the specific algorithm of the motion planner 310. In some embodiments of the present invention, a model predictive controller 340 (MPC) is used in the vehicle control layer so that future information in the long-term motion plan can be effectively used in the predictive controller to achieve the desired vehicle behavior.

[0068] In some embodiments of the present invention, a linear dynamic model is used in conjunction with linear constraints and a quadratic objective function, resulting in a linear model predictive controller (LMPC) tracking a reference motion calculated by a motion planner. In other embodiments of the present invention, one or more of the constraints and / or objective functions may be nonlinear, and / or the dynamic model equations describing the vehicle state behavior may be nonlinear, resulting in a nonlinear model predictive controller (NMPC) tracking a reference motion calculated by a motion planner.

[0069] Some embodiments of the present invention are based on the recognition that a motion planner can calculate a relatively long-term, highly predictive motion plan, but it typically needs to operate at a relatively slow sampling frequency. For example, a motion planner can calculate a reference motion for a future period of 5 to 50 seconds, while it can only execute once or multiple times within each sampling time of one or more seconds, resulting in a highly predictive motion plan, but relatively low responsiveness. Embodiments of the present invention are based on the additional recognition that a predictive controller can track a reference motion plan by calculating control signals within a relatively short prediction horizon, but while operating at a relatively high sampling frequency. For example, a vehicle controller can use a prediction horizon of 1 to 10 seconds, while it can execute 10 to 100 times per second. The vehicle controller can be highly responsive to local deviations caused by uncertainties in the vehicle state estimate and other uncertainties in the sensing and mapping modules (e.g., uncertainties associated with obstacles in the vehicle's surroundings).

[0070] In some embodiments of the invention, the execution rate of the adaptive model predictive controller 340 is greater than the execution rate of the probabilistic motion planner 311, such that the processor executes the MPC at least once for each execution of the motion planner. In some embodiments of the invention, the motion planner defines a sequence of parameter probability distributions with respect to a sequence of target states as a function of time over a period greater than the length of the prediction horizon of the vehicle controller.

[0071] In some embodiments of the present invention, different dynamic models of the vehicle may be used in different components of the multi-layer control and decision-making structure of an autonomous or semi-autonomous vehicle. For example, a relatively simple but computationally inexpensive kinematic model may be used in the motion planner, while a relatively accurate but computationally more expensive dynamic single-track or dual-track vehicle model may be used in the predictive controller.

[0072] like Figure 3A and Figure 3B As illustrated, information can be shared between different components in a multi-layered control and decision-making structure for an autonomous or semi-autonomous vehicle. For example, information related to a map and the vehicle's surroundings can be shared between the decision maker and the motion planner 305, between the motion planner and the vehicle controller 315, or between the vehicle controller and the actuator controller. In addition, some embodiments of the present invention are based on the recognition that the reliability and safety of an autonomous or semi-autonomous vehicle can be improved by using diagnostic information (e.g., performance metrics of success and / or failure of an algorithm in one component of the multi-layered control and decision-making structure that can be shared with algorithms in another component).

[0073] Reference Figure 3B , the probabilistic motion planner 311 can use information 315 from the model predictive controller 340. For example, in one embodiment of the invention, the MPC controller solves a constrained dynamic optimization problem at each sampling time step, and it uses the active set of constraints in each control solution at each sampling time step to provide feedback to the probabilistic motion planner. All constraints in the active set remain equal in the control solution, while all remaining constraints are considered inactive in the solution of the MPC controller. In some embodiments of the invention, the probabilistic motion planner 311 is configured to adjust the higher order moments of the probability distribution based on the type and / or number of active constraints in the MPC controller 340. This may be beneficial, for example, when the behavior of the motion planner needs to be adjusted to changes in the environment that are not or have not yet been detected by the motion planner, in order to improve the overall behavior of the autonomous or semi-autonomous vehicle.

[0074] Figure 3CThe invention illustrates the automatic tuning of one or more items of a cost function 350 of a model predictive controller 340 in a multi-layer control structure based on reference commands 105 and corresponding confidence levels 106 from a probabilistic motion planner 311. The adaptive MPC controller 340 is configured to optimize the cost function over a prediction horizon to generate a sequence of control commands for one or more actuators of a vehicle. In some embodiments of the invention, the optimization of the cost function 350 balances a cost 355 of tracking a target state sequence, defined by a first moment of a parameter probability distribution 316, against a cost 360 of at least one other metric of the vehicle's motion.

[0075] Embodiments of the present invention are based on the recognition that the importance of tracking costs can be weighted using a function of one or more of the higher-order moments of the parameter probability distribution 317. This automatic tuning of the cost function 350 in the adaptive predictive controller 340 based on the higher-order moments 317 of the parameter probability distribution in the probabilistic motion planner 311 allows for sharing the burden between the motion planner and the vehicle controller in achieving control goals (e.g., ensuring safe obstacle avoidance, staying in a specific lane, driving at a desired reference speed, and / or achieving reliable, comfortable driving behavior in relatively complex scenarios).

[0076] The high-order moments 317 of the probability distribution indicate the confidence of the probabilistic motion planner regarding the vehicle's motion plan. In some embodiments of the present invention, the adaptive predictive controller increases the weight of tracking in the balance optimization as the confidence increases, thereby allowing for lower deviations of the predicted vehicle state values ​​from the reference sequence of target states. In some embodiments of the present invention, the adaptive predictive controller decreases the weight of tracking in the balance optimization as the confidence decreases, thereby allowing for larger deviations of the predicted vehicle state values ​​from the reference trajectory.

[0077] In some embodiments of the present invention, a covariance matrix Pk 306 is used to represent the second-order moments of the probability distribution in the probabilistic motion planner. The time-varying sequence of this covariance matrix Pk 306 can be used to automatically tune the weighting of the time-varying reference tracking term 355 in the objective function of the adaptive MPC controller relative to other performance metrics 360. Some embodiments of the present invention are based on the recognition that this results in an automatic tuning mechanism for trading off between multiple competing control objectives. The confidence 106 associated with the reference trajectory of the target state and / or output value 105 in the terms of the covariance matrix Pk 306 indicates the degree to which the motion planner believes that its calculated trajectory is valid. By using the confidence information for automatically tuning the MPC cost function 350, we allow more deviations from the reference trajectory when the motion planner's confidence is low, and less deviations when this confidence is relatively high.

[0078] Figure 4A An exemplary list of possible control objectives for vehicle behavior while driving on a road and their corresponding control functions in a probabilistic motion planner according to some embodiments is shown. The control function transforms the vehicle's current state into a target state for the vehicle according to the corresponding objective. The control function can be designed analytically and / or learned from data. For example, one driving objective is to require the vehicle to stay on the road 400, and its corresponding stay-on-road function is configured to maintain the vehicle's position within the road boundaries. Possible additional driving objectives may require that the vehicle should travel in the middle 410 of the lane at a nominal speed 420. Their corresponding control functions may include: a lane centering function configured to maintain the vehicle's position in the middle of the lane, and / or a maintain speed function configured to maintain the vehicle's desired speed.

[0079] In another example, the driving objective may also require the vehicle to maintain a safety margin from surrounding obstacles 430 using its corresponding safety margin function, which is configured to maintain a minimum distance between the vehicle and obstacles on the road. Another possible driving objective is to maintain a safe distance from vehicles in the same lane 440. This can be achieved using a corresponding minimum-headway function, which is configured to maintain a minimum headway between the vehicle and the vehicle ahead. For reasons of passenger comfort, fuel consumption, wear, or other reasons, some embodiments of the present invention aim to require smooth driving behavior of the vehicle 450. Some embodiments implement this objective in the motion planner by using a smooth driving function configured to maintain smoothness of vehicle motion.

[0080] Other examples of driving objectives in a probabilistic motion planner may include: increasing the speed to a speed limit 460 using a speed limit function configured to maintain the speed of the vehicle at the speed limit; changing lanes 470 using a change lane function configured to change the current position of the vehicle from a current lane to an adjacent lane; and minimizing idling at intersections to reduce fuel consumption by using an intersection-crossing function configured to reduce the idling time of the vehicle at the intersection.

[0081] Some embodiments of the present invention are based on the recognition that a motion planner may have countervailing driving objectives. For example, it may be impossible to maintain a constant speed 420 while maintaining a safety margin 430 for surrounding obstacles. Some embodiments balance countervailing driving objectives by making at least one of the control functions probabilistic.

[0082] Specifically, some embodiments are based on the recognition that not all driving goals can be achieved precisely. For example, the goal of accelerating to a speed limit 460 may sometimes be incompatible with the driving goal of maintaining a safety margin 430 to surrounding obstacles. Moreover, the driver may decide which driving goal is most important in different ways from time to time. Moreover, in the case of self-driving vehicles, there is additional uncertainty that causes the driving goal to not be achieved precisely. Therefore, some embodiments of the present invention are based on the recognition that there is imprecision in achieving driving goals and that the degree to which such driving goals are achieved can change frequently.

[0083] Figure 4B A schematic diagram of the structure of a probabilistic control function 415 employed by some embodiments of a motion planner is shown. Each control function 415 is configured to probabilistically transition a vehicle's current state to a target state based on the control objective corresponding to the control function. To this end, the probabilistic control function 415 includes a deterministic component 425 for transitioning the current state to a target state 455 and a probabilistic component 435 for determining a probability distribution 465 of values ​​around the target state determined by the deterministic component. The deterministic component can generate a single value 445 of the target state or a sequence 455 of multiple values. Additionally or alternatively, the deterministic component can be executed multiple times to generate the sequence 455. However, in some implementations, for each value 445 of the sequence of target states, a probability distribution 465 exists such that the output of the probabilistic control function is a parametric probability distribution 475 about the target state defined by a first-order moment and at least one higher-order moment.

[0084] Figure 4C An example of the effect of the probability output of the motion planner 310 on the performance of the vehicle controller 320 is shown in accordance with some embodiments. In this example, Figure 4C The current state 410c of the vehicle and the target state 420c that the vehicle is intended to reach are shown. The target state 420c can be a Cartesian position, speed, or another entity associated with the vehicle. The target state 420c can be a specific state, or a range of states. For example, the target state 420c can be an interval of allowed speeds or a range of possible positions in Cartesian space. Suitable control inputs are to bring the vehicle from its initial state 410c to the target state 420c while achieving, for example, Figure 4A430c (e.g., the driving goal may be to stay within area 431c on the road). The resulting state 430c from the input may correspond to a state transition that results in the application of the control input one-time step, or the resulting state 430c may be connected to the initial state 410c using a trajectory 411c, i.e., a state transition sequence.

[0085] In the case of an autonomous or semi-autonomous vehicle, a transition from a current state to a target state can be performed by testing control inputs to a dynamic model of the vehicle's motion. The model of motion transitions the vehicle's state based on the control inputs submitted to the model. In various embodiments, the mathematical model of the vehicle's motion includes uncertainty. To this end, the model of the vehicle's motion is a probabilistic motion model to account for the fact that the model is a simplified description of the vehicle's actual motion and to account for uncertainty in the perception of the vehicle's true state, uncertainty in the perception of obstacle states, and uncertainty in the perception of the environment.

[0086] Figure 4C 431c is a target state region resulting from a particular selection of control inputs, wherein 431c is a non-zero probability component and 430c is a deterministic component included in region 431c. Driving target region 420c does not overlap with target state region 431c, i.e., region 431c does not include driving target region 420c. Therefore, referring to Figure 4C , the driving goal 420c is likely unattainable, and the controller may change its operation to better achieve one or more driving goals.

[0087] Figure 4D 431d is a target state region resulting from a particular selection of control inputs, wherein 431d is a non-zero probability component and 430d is a deterministic component included in region 431d. Driving target region 420d is smaller than target state region 431d and is entirely included in 431d, i.e., region 431d includes region 420d. Therefore, referring to Figure 4D , driving goal 420d is likely to be achieved through the selection of control inputs.

[0088] Figure 4E An illustration of a method of balancing the importance of different control functions according to some embodiments is shown. Figure 4EThe diagram illustrates a scenario with two driving objectives expressed as control functions: maintaining a nominal speed 420 and maintaining a safe distance 430. The speed requirement is expressed as a function of the speed's deviation from the nominal speed, and the safe distance is expressed as the Euclidean distance from the autonomous vehicle to an obstacle on the road. The deterministic component of the speed control function is zero, while the probabilistic component is a narrow shape 410e centered around the deterministic component. On the other hand, the safe distance requirement has a non-zero deterministic component 420e and a probabilistic component 430e that allows for large variations around the deterministic component. Moreover, in this illustrative example, the deterministic component 420e is not centered around the mean of the probabilistic component 430e. Because speed and distance are different, the speed and safe distance control functions are transformed 440e into a common state in which the control functions can be compared. The control functions are then combined 450e into a joint distribution that weights the control functions together. In some embodiments of the present invention, the relative scaling of different control objectives is used to combine and balance the relative importance of the control functions.

[0089] For example, in Figure 4E , the joint distribution balances the importance 450e into a common deterministic component 470e and a probabilistic component 460e, both of which are closer to the corresponding components of the speed control function because the probability distribution of the speed control function has much smaller variation 410e than the probabilistic component 430e of the safety distance control function.

[0090] The joint distribution can be chosen in a variety of ways. For example, if the probability components of the control functions of each step in the sequence are Gaussian, then the joint distribution can be chosen to be a multivariate Gaussian distribution where the importance of each control function is weighted by the inverse of the covariance of the components.

[0091] The deterministic components can be selected in a variety of ways. For example, one embodiment combines the deterministic components by stacking them on a vector such that they constitute the average of a Gaussian distribution.

[0092] Even if the probabilistic components of the individual steps in a sequence are Gaussian, the sequence distribution can be non-Gaussian, especially when combined to form a joint distribution sequence. For example, the deterministic component can be a nonlinear function that maps the current state to the output of the control function, making the sequence non-Gaussian. To determine the combined state and distribution sequence in this case, numerical approximation can be used, for example, through sampling.

[0093] Some embodiments of the present invention are based on the recognition that a similar balance of different control objectives can be used in an adaptive predictive controller that is designed to track a reference motion calculated by a probabilistic motion planner. For example, the tracking of different state variables can be combined into a joint tracking cost function, where a weighting matrix is ​​used to balance the importance of tracking one of the state variables relative to the importance of tracking one or more other state variables. In some embodiments of the present invention, the weighting matrix is ​​calculated as a function of a scaling matrix and one or more higher-order moments 217 of a sequence of parameterized probability distributions over a sequence of target states and / or output values ​​that define the motion commands for the vehicle calculated by the probabilistic motion planner 311. Some embodiments include a saturation function that bounds each of the weighting matrices between a lower bound and an upper bound on the weighting of the respective one of the control objectives in the joint cost function.

[0094] In addition to tracking different state variables, some embodiments of the present invention also include one or more additional objective terms to be considered by the predictive controller. Examples of these additional terms may relate to driving comfort, speed limits, energy consumption, pollution, etc. These embodiments utilize these additional objective terms to balance the cost of tracking reference values ​​for the different state variables.

[0095] Figure 5A A block diagram of a system and method for implementing a controller 110 using model predictive control (MPC) according to some embodiments is shown, which computes a control signal 111 given a current state 121 of the system and a control command 101. Specifically, MPC computes a control solution 560, e.g., a solution vector 555, containing a sequence of future optimal control inputs for a prediction time horizon of the system by solving an inequality-constrained optimization problem 550 in the form of an optimal control structured program at each control time step. The objective function 540 and the optimal control data 545 of the equality and inequality constraints 530 in the optimization problem 550 depend on a dynamic model 525, system constraints 520, the current state 121 of the system, and the control command 101 consisting of a reference 105 and a confidence 106.

[0096] In some embodiments, the solution 550 to the inequality-constrained optimization problem uses the state and control values ​​510 from the prediction time horizon of the previous control time step, which can be read from memory. This concept is called a warm start or hot start of the optimization algorithm, and in some embodiments it can significantly reduce the computational workload required by the MPC controller. In a similar manner, the corresponding solution vector 555 can be used to update and store the sequence 560 of optimal or suboptimal state and control values ​​for the next control time step.

[0097] In some embodiments of the invention, the MPC controller 340 adapts one or more of the optimal control cost function 540 to the reference trajectory 105 and the corresponding confidence bounds 106 calculated by the probabilistic motion planner 311. Figure 5B As shown, a least squares type cost function can be used to track a reference trajectory with a particular weighting matrix. In some embodiments of the invention, the reference trajectory is defined by first-order moments 216, and the weighting matrix is ​​calculated as a function of the covariance matrix 306 or a function of one or more higher-order moments 217 of a sequence of parameterized probability distributions about a sequence of target states and / or output values ​​that define the motion commands for the vehicle calculated by the probabilistic motion planner 311.

[0098] Figure 6A A block diagram is shown of a system and method for implementing an adaptive MPC 340 to implement a vehicle controller 110, according to some embodiments, by solving an optimal control structured quadratic program (QP) 650 to compute a control signal 111 given the current state 121 of the system and a control command 101. In some embodiments of the invention, the adaptive MPC controller uses a linear quadratic objective function in conjunction with a linear dynamic model to predict the behavior of the vehicle and linear inequality constraints, resulting in an optimal control structured QP problem formulation as follows:

[0099]

[0100]

[0101] x k+1 =a k +A k x k +B k u k , k=0,...,N-1,

[0102]

[0103]

[0104]

[0105] In the case where the prediction horizon of the adaptive linear MPC controller is discretized in time, the discrete time points t k (where k=0, ..., N) is divided into N equidistant or non-equidistant control interval sequences. The optimization variables in the optimal control structured QP 650 are determined by the state variables x k and control input variable uk (where k=0, ..., N). In some embodiments of the present invention, for each discrete time point t k (where k=0, ..., N), the dimensions of the state and control variables do not need to be equal to each other. At each sampling time of the adaptive MPC controller, the optimal control structure QP 650 is formulated using the QP matrix 630 and the QP vector 635, and then the QP is solved to calculate the solution vector 555 to update the state and control trajectory 560 and generate a new control signal 111.

[0106] The objective function in the constrained QP 650 solved by the adaptive MPC controller 340 includes one or more least squares reference tracking terms 652 that penalize the difference between the sequence of predicted state and / or output values ​​656 and the sequence of reference state and / or output values ​​105 calculated by the motion planner. The reference state and / or output values ​​105 define the first moment 316 of the probability distribution.

[0107] In some embodiments of the present invention, the weight matrix W k The sequence of is used in the least squares reference tracking term 652 (where k = 0, ..., N), and the weight matrices W are adapted in the control cost function 640 based on the reference 105 and confidence 106 calculated by the probabilistic motion planner at each sampling instant. k . Weighting matrix W k is calculated as a function of or represented by the higher-order moments 317 of the probability distribution.

[0108] Output variable y used in reference tracking target item 652 k (where k = 0, ..., N) can be defined as any linear function of the state and / or control input variables 656. For example, the output function may include one or more combinations of one or more of the vehicle's longitudinal or lateral velocity and / or acceleration, slip rate or slip angle, orientation angle or angular velocity, wheel speed, force and / or torque. The reference tracking target term 652 is a weighted matrix W in the QP matrix 630. k and the reference value in the QP vector 635 In some embodiments of the present invention, the weight matrix is ​​positive definite W k >0 or positive semidefinite W k ≥0, that is, the matrix W k All eigenvalues ​​of are greater than zero, or the matrix W k All eigenvalues ​​of are greater than or equal to zero.

[0109] In various embodiments, the penalty between the reference value determined by the motion planner and the value determined by the predictive controller is weighted by a weighting matrix that assigns different weights to different state variables of the target state. Additionally or alternatively, some embodiments add additional objective terms to be considered by the predictive controller. Examples of these additional terms may relate to driving comfort, speed limits, energy consumption, pollution, etc. These embodiments utilize these additional objective terms to balance the costs of reference tracking.

[0110] For example, some embodiments define additional objective terms in the MPC cost function in the form of linear quadratic stage costs 651 and / or linear quadratic terminal cost terms 653. These additional linear quadratic objective terms, including the stage costs 651 and terminal costs 653, may include linear and / or quadratic penalties for one or more combinations of one or more state and / or control input variables. For example, the objective function in the constraint QP 650 may include linear or quadratic penalties for the longitudinal or lateral velocity and / or acceleration of the vehicle, slip rate or slip angle, orientation angle or angular velocity, wheel speed, force, torque, or any combination of these quantities. The linear quadratic objective terms in the stage costs 651 and terminal costs 653 are generated by the matrix Q in the QP matrix 630. k 、S k and R k and the gradient value q in the QP vector 635 k 、r k In some embodiments of the present invention, the Hessian matrix It is positive H k >0 or positive semidefinite H k ≥, that is, the Hessian matrix H k All eigenvalues ​​of are greater than zero, or the matrix H k All eigenvalues ​​of are greater than or equal to zero.

[0111] The constrained optimal control structured QP 650 solved by the adaptive linear MPC controller 340 defines a linear dynamic model 655 that describes the behavior of the system at a given previous time step t k Under the condition of state and control variables, at a time step t k+1 The linear dynamic model is composed of the matrix A in the QP matrix 630. k and B k The time-invariant sequence or time-varying sequence and the vector a in the QP vector 635 k (where k=0, ..., N-1). Given the current state estimate in the equality constraint for the initial state value 654 121 and control input value u kThe linear dynamic model equation 655 can be used to calculate the state value x k (where k=0, ..., N).

[0112] In addition to the equality constraints including initial value conditions 654, dynamic equations 655, and output equations 656, the constrained optimal control structured QP 650 may also include one or more inequality constraints to impose physical limitations of the system, safety constraints, and / or desired performance-related constraints on the behavior of the autonomous or semi-autonomous vehicle. More specifically, the QP may include a path inequality constraint 657 (where k = 0, ..., N-1) and / or a terminal inequality constraint 658 at the end of the prediction horizon. The inequality constraints are defined by the matrix in the QP matrix 630. and The time-invariant sequence or time-varying sequence and the vector d in the QP vector 635 k (where k=0, ..., N).

[0113] The inequality constraints may include one or more combinations of constraints on one or more of the vehicle's longitudinal or lateral velocity and / or acceleration, the vehicle's position and / or orientation relative to its surroundings, slip rate or slip angle, orientation angle or angular velocity, wheel speed, force, and / or torque. For example, obstacle avoidance constraints may be implemented in an adaptive MPC controller by defining a set of one or more inequality constraints on linear functions of the vehicle's predicted position, velocity, and orientation relative to the predicted position, velocity, and orientation of one or more obstacles in the vehicle's surroundings.

[0114] Some embodiments of the present invention are based on the recognition that if the Hessian matrix H k 651. Terminal Cost Matrix Q N 653 and weight matrix W k 652 is positive definite or positive semidefinite, then the optimal control structured QP 650 is convex. Embodiments of the present invention may use an iterative optimization algorithm to solve the optimal control structured QP 650 to find a solution vector 555 that is feasible with respect to the constraints and is globally optimal, or feasible but suboptimal, or the algorithm may find a low-precision approximate control solution that is neither feasible nor optimal. As part of the adaptive MPC controller, the optimization algorithm may be implemented in hardware or as a software program executed in a processor.

[0115] Examples of iterative optimization algorithms for solving QP 650 include: primal or dual gradient-based methods, projected or approximate gradient methods, forward-backward segmentation methods, alternating direction methods of multipliers, primal, dual, or primal-dual active set methods, primal or primal-dual interior point methods, or variations of these optimization algorithms. In some embodiments of the present invention, block-sparse optimal control structures in QP matrix 630 may be utilized in one or more of the linear algebra operations of the optimization algorithm to reduce computational complexity, and therefore reduce execution time and memory footprint, of the QP optimization algorithm.

[0116] Other embodiments of the present invention may use an optimization algorithm for nonlinear programming (e.g., sequential quadratic programming (SQP) or interior point method (IPM)) to solve the nonconvex optimal control structured QP 650. The optimization algorithm may find a suboptimal, locally optimal, or globally optimal control solution to the inequality constrained optimization problem at each sampling time of the adaptive MPC controller 340.

[0117] Figure 6B A block diagram is shown of a system and method for implementing an adaptive MPC 340 to implement a vehicle controller 110 that computes a control signal 111 by solving an optimal control structured nonlinear program (NLP) 660 given the current state 121 of the system and the control command 101. In some embodiments of the present invention, the adaptive MPC controller uses a linear quadratic or nonlinear objective function in conjunction with a linear or nonlinear dynamic model to predict the behavior of the vehicle and a combination of linear and nonlinear inequality constraints, resulting in an optimal control structured NLP problem formulation that can be expressed as:

[0118]

[0119]

[0120] x k+1 =F k (x k ,u k ), k=0,...,N-1,

[0121]

[0122] 0≥h k (x k ,u k ), k=0,...,N-1,

[0123] 0≥h N (x N ),

[0124] In the case where the prediction horizon of the adaptive nonlinear MPC controller is discretized in time, the discrete time points t k The optimization variables in the optimal control structured NLP 660 are determined by the state variables x k and control input variable u k (where k=0, ..., N). In some embodiments of the present invention, for each discrete time point t k (where k=0, ..., N), the dimensions of the state and control variables do not need to be equal to each other. At each sampling time of the adaptive MPC controller, the optimal control structure NLP 660 is formulated using the reference and weight matrices in the reference tracking cost 641 and the NLP objective and constraint function 645, and the NLP is solved to calculate the solution vector 555 to update the state and control trajectory 560 and generate a new control signal 111.

[0125] The objective function in the constraint NLP 660 solved by the adaptive MPC controller 340 includes one or more linear and / or nonlinear least squares reference tracking terms 662 that penalize the difference between the sequence of predicted state and / or output values ​​and the sequence of reference state and / or output values ​​105 calculated by the motion planner. In some embodiments of the invention, the weight matrix W is set to k The sequence of is used in the least squares reference tracking term 662 (where k = 0, ..., N), and the weight matrices W are adapted in the control cost function 640 based on the reference 105 and confidence 106 calculated by the probabilistic motion planner at each sampling instant. k The output value y used in the reference tracking target item 662 k (x k ,u k ) (where k = 0, ..., N) can be defined as any linear or nonlinear function of state and / or control input variables. For example, the output function may include one or more combinations of one or more of the vehicle's longitudinal or lateral velocity and / or acceleration, slip rate or slip angle, orientation angle or angular velocity, wheel speed, force and / or torque. The reference tracking target term 662 is a matrix of weights W k and reference values In some embodiments of the present invention, the weight matrix is ​​positive definite W k >0 or positive semidefinite W k ≥0, that is, the matrix W k All eigenvalues ​​of are greater than zero, or the matrix W k All eigenvalues ​​of are greater than or equal to zero.

[0126] Embodiments of the present invention may define additional objective terms in the MPC cost function in the form of stage cost and / or terminal cost terms 663, both of which may be composed of any combination of linear, linear-quadratic, or nonlinear functions. These additional objective terms may include penalties for one or more combinations of one or more linear or nonlinear functions of state and / or control input variables. For example, the objective function 645 in the constraint NLP 660 may include linear, quadratic, or nonlinear penalties for the vehicle's longitudinal or lateral velocity and / or acceleration, slip rate or angle, orientation angle or angular velocity, wheel speed, force, torque, or any combination of these quantities.

[0127] The constrained optimal control structured NLP 660 solved by the adaptive nonlinear MPC controller 340 may define a nonlinear dynamic model 665 that describes the behavior of the system at a given previous time step t k Under the condition of state and control variables, at a time step t k+1 The vehicle state at the location. The nonlinear dynamic model is composed of a time-invariant or time-varying function x k+1 =F k (x k ,u k )(where k=0, ..., N-1). The current state estimate given the equality constraint of the initial state value 664 121 and control input value u k In the case of a sequence of k (where k=0, ..., N).

[0128] Some embodiments of the present invention are based on the recognition that a discrete-time dynamic model 665 for predicting vehicle behavior can be obtained by performing a time discretization of a set of continuous-time differential or differential-algebraic equations. This time discretization can sometimes be performed analytically, but typically requires the use of numerical simulation routines to compute numerical approximations of the discrete-time evolution of the state trajectory. Examples of numerical routines that approximate a set of continuous-time differential or differential-algebraic equations include explicit or implicit Runge-Kutta methods, explicit or implicit Euler methods, backward differentiation formulas, and other single-step or multi-step methods.

[0129] In addition to the equality constraints including initial value conditions 664 and dynamic equations 665, the constrained optimal control structured NLP 660 may also include one or more linear and / or nonlinear inequality constraints that impose physical limitations of the system, safety constraints, and / or desired performance-related constraints on the behavior of the autonomous or semi-autonomous vehicle. More specifically, the NLP may include linear path inequality constraints 666 or nonlinear path inequality constraints 667 (where k = 0, ..., N-1), as well as linear and / or nonlinear terminal inequality constraints 668 imposed at the end of the prediction horizon. The inequality constraints are given by the matrix Time-invariant or time-varying sequence, vector d k and / or nonlinear function h k (x k ,u k )(where k=0, ..., N).

[0130] The inequality constraints may include constraints on one or more combinations of one or more of the longitudinal or lateral velocity and / or acceleration of the vehicle, the position and / or orientation of the vehicle relative to its surroundings, slip rate or slip angle, orientation angle or angular velocity, wheel speed, force, and / or torque. For example, obstacle avoidance constraints may be implemented in an adaptive nonlinear MPC controller by defining a set of one or more inequality constraints on linear or nonlinear functions of the vehicle's predicted position, velocity, and orientation relative to the predicted position, velocity, and orientation of one or more obstacles in the vehicle's surroundings.

[0131] In some embodiments of the present invention, the adaptive nonlinear MPC controller includes an obstacle avoidance constraint implemented using a time-varying sequence of one or more elliptic inequality constraints, which can be read as:

[0132]

[0133] in, is the estimated or predicted vehicle position (p X , p Y ) to the estimated and / or predicted position of one of the potential multiple obstacles in the vehicle's surroundings. The position and orientation of each obstacle is given by (e x，j , e y，j , e ψ，j ) is represented by the matrix R(e ψ，j ) T represents the angle e with respect to the obstacle ψ，j The corresponding rotation matrix is ​​the transpose, and (a x，j , a y，j) represents the length of the major semi-axis of the ellipsoid that defines the safety margin around each of the M (where j=1, ..., M) nearest detected obstacles, including the uncertainty around the spatial extent of the estimated shape. Real-time obstacle detection and corresponding pose estimation can be performed by the sensing module, and this information is shared by different components in the multi-layer vehicle control architecture.

[0134] Some embodiments of the present invention are based on a customized optimization algorithm to efficiently solve the constrained optimal control structure NLP 660 at each sampling time of the nonlinear adaptive MPC controller. Such an optimization algorithm can find a solution vector 555 that is feasible with respect to the constraints and is globally optimal, feasible but locally optimal, feasible but suboptimal, or an iterative optimization algorithm can find a low-precision approximate control solution that is neither feasible nor locally optimal. Examples of NLP optimization algorithms include variants of interior point methods and variants of sequential quadratic programming (SQP) methods.

[0135] In particular, some embodiments of the present invention use a real-time iteration (RTI) algorithm, which is an online variant of sequential quadratic programming combined with a quasi-Newton or generalized Gauss-Newton type semi-positive Hessian approximation, such that at each sampling instant of the nonlinear MPC controller, at least one convex sparse QP approximation needs to be solved. Each RTI iteration consists of two steps:

[0136] (1) Preparation phase: Discretize and linearize the system dynamics, linearize the remaining constraint functions, and evaluate the quadratic objective approximation to construct the optimal control structure QP subproblem.

[0137] (2) Feedback phase: Solve the QP to update the current values ​​of all optimization variables and obtain the next control input to apply feedback to the system.

[0138] In some embodiments of the present invention, block sparse optimal control structures in the Hessian matrix and the constrained Jacobian matrix can be utilized in one or more of the linear algebra operations of the optimization algorithm to reduce the computational complexity and thereby reduce the execution time and memory footprint of the NLP optimization algorithm.

[0139] Figure 7AA block diagram is shown of a method for adapting a reference and weighting matrix 640 in a particular embodiment of a time-varying reference tracking cost 720 in an adaptive linear or nonlinear MPC controller 340. The MPC tracking cost 720 is weighted using a time-varying positive definite or semi-positive definite weighting matrix 710, and each of the matrices in the weighting matrix is ​​calculated based on an inversely proportional relationship to the time-varying uncertainty around the reference, which is represented by a combination of one or more high-order moments 317 of the parameter probability distribution as calculated by the probabilistic motion planner.

[0140] In some embodiments of the present invention, MPC uses a time-varying positive definite or positive semidefinite weighting matrix in the tracking cost, which is calculated as the inverse of the staged scaling of the sequence of covariance matrices 705 of the parameter probability distribution from the probabilistic motion planner. The sequence of covariance matrices 705 represents or includes high-order moments of the probability distribution determined by the motion planner. The motion planner weights the different control objectives relative to their respective importance to generate a suitable sequence and probability distribution for MPC control of vehicle states and / or output values. Given the covariance matrix P corresponding to each reference state and / or output value, k > 0, the weight matrix in the adaptive linear or nonlinear MPC controller can be calculated as follows:

[0141]

[0142] Among them, W k The weight matrix in the MPC reference tracking cost 720 is represented by the matrix P k represents the covariance matrix 705, and Q k >0 is a symmetric and positive definite scaling matrix 701. The scaling matrix Q k is selected as a time-invariant or time-varying sequence of symmetric and positive definite matrices, wherein each scaling matrix is ​​a dense or diagonal matrix. In some embodiments of the invention, the scaling matrices are selected by the probabilistic motion planner based on the relative scaling of different control objectives with respect to their respective importance.

[0143] exist Figure 7A In the example above, we use the square root of the inverse of the covariance matrix To calculate the weight matrix for each control interval k=0, ..., N. In some embodiments of the present invention, the weight matrix in the adaptive linear or nonlinear MPC controller can be alternatively calculated as follows:

[0144]

[0145] Among them, L k represents the positive definite covariance matrix P k >0 Cholesky factor, so and Some embodiments are based on the recognition that the weighting matrix It can be based on the positive definite covariance matrix Cholesky factorization and positive definite scaling matrix Cholesky factor decomposition is used for efficient calculation, so that the intermediate matrix Forward or backward substitution can be used for efficient computation, and the symmetric weight matrix can then be computed as

[0146] Figure 7B It shows that based on the diagonal positive definite weighting matrix W k >0715, a block diagram of a method for adapting the reference and weighting matrix 640 in a specific embodiment of a time-varying reference tracking cost 720 in an adaptive linear or nonlinear MPC controller 340, wherein each of the diagonal entries

[0147] Where i = 1, ..., n x .

[0148] is to use only positive definite scaling matrices Q k >0701 and the corresponding diagonal entries and the reference covariance matrix P k In some embodiments of the present invention, one or more saturation functions can be used to improve the numerical regulation of constrained optimization problems solved by MPC, for example, based on regularization parameters ∈>0, so as to provide lower and upper bounds for each of the time-varying positive definite weight matrices in the reference tracking cost of the adaptive MPC controller. Representing the weight matrix as a diagonal matrix is ​​computationally cheaper.

[0149] Figure 7CAn algorithmic description of automatically tuning the time-varying reference tracking cost of an adaptive MPC controller 540 using high-order moments from a probabilistic motion planner is shown. Based on the sequence of reference state and / or output values ​​and the corresponding covariance matrix 740, a stationary approximation of the reference motion 743 can be used in a least-squares objective term of the MPC controller, and each of the weight matrices can be individually calculated based on an inversely proportional relationship to each of the covariance matrices 730 from the probabilistic motion planner, which is repeated at each sampling instant of the MPC controller 744. If a new reference motion plan is calculated 741 by the probabilistic motion planner (including the first-order moments 316 and high-order moments 317 of the parameter probability distribution), the sequence of reference state and / or output values ​​and the corresponding covariance matrix can be reset 742. If a new reference motion plan is not yet available, the most recent data sequence of reference state and / or output values ​​and the corresponding covariance matrix can be carried forward from one control time step to the next.

[0150] Some embodiments of the present invention are based on the recognition that a motion planner operates on longer time scales than an MPC, while an MPC controller can execute at a higher sampling rate than the motion planner, so that the MPC can adjust to environmental changes more quickly and adjust for uncertainties in the sensing and estimation of the vehicle state and the vehicle's surroundings compared to the relatively slow reactivity of the motion planner.

[0151] In some embodiments of the invention, the computation of the combined sequence of states and the probability distribution in the probabilistic motion planner is implemented as a tree that expands until a sequence of states that leads to a driving decision has been found.

[0152] Figure 8 A schematic diagram of a state transition tree defining vehicle motion according to some embodiments of the present invention is shown. The current tree in drivable space 830 is shown, where root node 800 indicates the current state of the vehicle, and the tree includes states as nodes and state transitions as edges in the state space, which are generated by control inputs selected according to other embodiments of the present invention. For example, edge 821 is a motion generated by applying a control input within a predefined time from root node 800 to state 820. The tree may include a target state 810 and a target area 840 for the vehicle. In some embodiments of the present invention, there may be multiple target states 810 and target areas 840. A probability may be associated with the control input that generated edge 821, and therefore also with state 820, which may account for uncertainty in the vehicle's dynamic model and uncertainty in the sensing and estimation of obstacles and the vehicle's surroundings.

[0153] In some embodiments, edge 821 is created by evaluating control inputs at multiple moments. Other embodiments determine new control inputs for each moment, wherein the determination of control inputs is described in accordance with other embodiments of the present invention. In other embodiments, edge 821 is created by aggregating multiple control inputs at one or more moments. As the tree is extended toward target area 840, an initial state is selected, control inputs are determined, and a corresponding sequence of states and a final state are determined. For example, 880 may be the selected state, 881 may be a trajectory (which is added to the tree as an edge), and 860 is the final state (which is added to the motion planning tree as a node).

[0154] Figure 9A A flow chart of a probabilistic motion planner 899 for determining a sequence and distribution of states is shown according to some embodiments of the present invention. In some embodiments, the probabilistic motion planner 899 is a particle filter that propagates a set of particles in time to represent a set of likelihoods of a target state at a time instance. Each particle comprises a Gaussian distribution of values ​​of the target state at a time instance, wherein the first order moment of the parameter probability distribution at the time instance is a weighted average of the particles and the higher order moments of the parameter probability distribution at the time instance are the weighted covariances of the particles. The method iteratively determines a sequence of control inputs that specify the motion of a vehicle from an initial state of the vehicle to a target state of the vehicle. In various embodiments, the initial state is the current state of the vehicle and / or wherein the initial state is a state corresponding to a control input determined during a previous iteration of the method.

[0155] The motion is defined by state transitions connecting vehicle states, e.g. Figure 8 Each state includes the vehicle's position, velocity, and direction. The motion is determined iteratively until a termination condition is met, for example, for a certain period of time or a predetermined number of iterations. Figure 9A The iteration of the method includes the following steps.

[0156] The method determines 900 an initial state, a set of sampled states, and a corresponding set of state transitions such that a particular state transition with a corresponding high probability is relatively consistent with a subset of the control objective function. For example, the method determines Figure 8 State 880, state transition 881, and state 860 in .

[0157] In some embodiments of the present invention, the sampled states 900 are generated by using a probabilistic control function, i.e., the states are sampled according to a probability density function corresponding to a subset of the control functions. For example, given a state at time index k and a control function at time index k+1, the probability function q(x k+1 |x k,y k+1 ) to generate the state, where q is a function of the state at time index k+1.

[0158] As a specific example, if the noise and control functions on the motion model are Gaussian, then the Gaussian density function q can be chosen as in, and That is, the states can be generated as random samples from a combination of noise sources and a probabilistic control function of the dynamic system.

[0159] In one embodiment of the present invention, the generation of the sampled state 900 is performed in a loop, wherein the number of iterations is predetermined. In another embodiment, the generation of the state 900 is performed based on a requirement of T time steps ahead in time. For example, the number of iterations T can be determined as a fixed number of steps, or the number of iterations can be determined as a function of the resolution of the sensors of the sensing system. When 900 is executed for T time steps, according to all probability control functions (i.e., q(x)) from time index k+1 to time index k+T, k+1 |x k ,y k+1 ,...,y k+T )) to generate input.

[0160] Figure 9B A flow chart of a method 910 for determining the probability of each state being consistent with a probabilistic control function is shown. When determining the probability of each state, the state is first checked for collisions 911. If the next state and the state transition leading to the state are collision-free, the consistency of the state with a subset of the control function is determined 912, and the probability of each state is calculated 913.

[0161] In one embodiment, if the collision check 911 determines that the next state Collision checking can be deterministic, or it can be probabilistic, where a collision is assumed to have occurred if the probability of a collision is above a certain threshold, where the prediction of the obstacle is made based on a probabilistic motion model of the obstacle.

[0162] Figure 9C An example is shown in which the prediction of the autonomous vehicle's position 910c intersects the uncertainty region 921c of an obstacle 920c, and the probability that the obstacle 920c is at position 910c is above a collision threshold 930c. For example, the vehicle's sensors may determine the position of the obstacle as a function of time. The motion planning system determines the probability that the next state will intersect the obstacle's uncertainty region and assigns a probability of zero to the sampled state when the probability that the next state intersects the obstacle's uncertainty region is above the collision threshold.

[0163] In another embodiment of method 910 , if the clustering probability is below a threshold 914 , where the threshold may be predetermined, the state has a low probability of being consistent with the control function, so the method exits 915 and re-enters the motion planning algorithm 899 .

[0164] In some embodiments of the invention, determining 912 is a probability density function (PDF) as a probability control function The next state, and the probability of the state determined during the previous cycle 960 For example, if the state is generated according to the dynamic model of the vehicle, the probability is proportional to the PDF of the control function, that is, As another example, if the state is sampled according to is done, then as explained above, the probability is proportional to the prediction of the PDF of the probability control function, that is, In some embodiments of the invention, the probabilities are normalized in a way that represents a PDF.

[0165] In one embodiment of the present invention, states with non-zero but low probability are replaced by states with higher probability in some time steps. For example, one embodiment generates a new set of states in such a way that The probability of In another embodiment, whenever the inverse sum of the squares of the probabilities is below a certain predefined threshold, a replacement is performed. Doing this in this way ensures that only states that are likely to be good are used.

[0166] The determination of the state 920 can be accomplished in a variety of ways. For example, one embodiment determines the control input using a weighted average function to generate the state as Another embodiment determines the state to be the state with the highest probability, i.e., Additionally or alternatively, one embodiment determines the state by averaging a fixed number m<N of sampled states.

[0167] Figure 9DA simplified schematic diagram illustrates the results of three iterations of steps 900, 910, and 920, when five sampled states are generated per iteration. Using a dynamic model of vehicle motion and a probabilistic control function, an initial state 910d is predicted forward at time 911d, with the next five states being 921d, 922d, 923d, 924d, and 925d. Probabilities are determined as a function of a probabilistic control function 926d and a probabilistic tolerance 927d for the control function 926d. At each time step, i.e., at each iteration, a set of probabilities is used to generate a set of control inputs and corresponding states 920d. In some implementations, the probability function acts to propagate particles representing the likelihood of a target state over time. In these implementations, each state (e.g., 921d, 922d, 923d, 924d, and 925d) is a particle.

[0168] Figure 9E Shown Figure 9D The possible assignment probabilities of the five states at the first iteration in . The values ​​of these probabilities 921e, 922e, 923e, 924e, and 925e are reflected in the relative sizes of the points selected to illustrate states 921d, 922d, 923d, 924d, and 925d.

[0169] Determining the probability distribution of a sequence means determining the distribution of probabilities, such as Figure 9E For example, the distribution can be expressed as Figure 9E Discrete distributions in , or discrete states associated with probabilities can be made continuous using, for example, kernel density smoothers.

[0170] Return to reference Figure 9D , state 920d becomes the initial state for the next iteration, which again generates five sampled states 931d, 932d, 933d, 934d, and 935d. State 930d is selected based on the probability of the sampled states for this iteration. State 930d is the initial state for the next iteration.

[0171] Some embodiments update the tree of nodes and edges G = (V, E) 930 as follows. If this is the first iteration of method 900, the tree is initialized with the current state and the edges are empty. Otherwise, the sequence of aggregate states and control input sequences determined in 900-920 are added as nodes, and the traces connecting the states are added as edges. For example, Figure 8 860 in is the added node, and the corresponding edge is 881. Alternatively, in one embodiment, all generated states are added to the tree, in which case determination 920 can be avoided.

[0172] 10A to 10DA diagram showing a subset of possible control functions and their corresponding deterministic and probabilistic components according to some embodiments of the present invention is shown.

[0173] Figure 10A An example diagram of a control function corresponding to a control objective of keeping vehicle 1010a on road 400 is shown, wherein the road boundary is defined by 1020. The control function is defined by a deterministic component 1030a and a probabilistic component 1040a. For example, the deterministic component can be determined by recording data of a human driver and optimizing the fit (e.g., by minimizing the average Euclidean distance for the data or by maximizing the probability of being a good fit). The probabilistic component 1040a can be determined by determining the variation of the recorded data around the deterministic component 1030a. For example, the probabilistic component 1040a can be determined by maximizing the probability that all recorded data is included within the variation of the probabilistic component 1040a, or by estimating the actual variation of an unlimited amount of data given a limited amount of recorded data.

[0174] Some embodiments are based on the recognition that while the control function can be modeled as being bounded by the road boundary 1020a, this is not how a human drives. Instead, a human may decide to take turns cutting corners to provide a shorter ride. Figure 10B An example diagram illustrates a control function corresponding to a control objective of reducing the travel time of a vehicle 1010b on a road, where the road boundary is defined by 1020b. The control function is defined by a deterministic component 1030b and a probabilistic component 1040b. According to some embodiments of the present invention, a reference tracking cost in a linear or nonlinear MPC controller can be automatically adapted based on time-varying first-order moments and higher-order moments of a probability distribution calculated by a probabilistic motion planner using the deterministic component 1030b and the probabilistic component 1040b for each of the control objectives.

[0175] Figure 10C An example diagram of a control function expressing a driving goal of safely passing an obstacle 1060c is shown, where a vehicle 1010c is traveling on a two-lane road with lane boundaries 1070c. Figure 10CA scenario is illustrated in which an autonomous or semi-autonomous vehicle needs to perform an obstacle avoidance maneuver. Performing an obstacle avoidance maneuver may be necessary when it is likely that the vehicle cannot safely stay in the middle of the current lane due to static or dynamic obstacles (e.g., pedestrians, bicycles, and stopped or parked vehicles) on the sides of the current lane, and when it is impossible or undesirable to change to another lane, for example, due to traffic regulations or when the other lane is currently blocked. For example, the deterministic component 1020c can be determined based on data collected from human drivers in similar situations. The probabilistic component 1040c indicates that not every situation is the same, and that the driver typically behaves slightly differently depending on how the obstacle 1060c behaves.

[0176] Thus, the probabilistic component 1040c indicates a large change in the area where the overtaking is performed. In some embodiments of the present invention, such a large change in the area where the overtaking is performed can result in a corresponding reduction in the weight of the reference tracking cost term of the adaptive MPC controller. This allows the MPC reference tracking algorithm to automatically adapt to the trade-offs between competing control objectives, such as achieving high tracking performance while meeting safe obstacle avoidance requirements. More specifically, the uncertainty of the reference motion plan can be expected to increase as the vehicle is predicted to become relatively close to surrounding obstacles, so that the penalty for deviating from the reference trajectory will be reduced, and vice versa. This in turn allows the NMPC trajectory to deviate more or less from its reference (i.e., the motion plan trajectory).

[0177] Figure 10D An example diagram is shown of a possible way to describe a control function to maintain a target of current speed 420 according to some embodiments. Figure 10D The case where the speed is modeled according to the road curvature (i.e., a measure of the turning radius of the road) is shown. For a small curvature, i.e., an almost straight road, the control function has a deterministic component 1020d equal to the current speed 1010d. However, as the curvature increases, the current speed becomes more difficult to maintain, and the deterministic component decreases to zero. Moreover, as Figure 10D As shown, for small curvatures, the probabilistic component 1030d is centered around the nominal speed with small variations. However, as the curvature increases, there is a larger asymmetric variation to reflect the larger changes in speed imposed by the driver. In some embodiments of the present invention, for large curvatures of the road, such large changes in the desired speed may result in a corresponding decrease in the reference tracking cost term of the adaptive MPC controller. For small curvatures of the road, smaller changes in the desired speed may result in a corresponding increase in the reference tracking cost term of the MPC controller.

[0178] The above-described embodiments of the present invention can be implemented in any of a variety of 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 across multiple computers. Such a processor can be implemented as an integrated circuit, with one or more processors in the integrated circuit assembly. However, the processor can be implemented using circuitry of any suitable format.

[0179] Furthermore, the various methods or processes outlined herein may be encoded as software executable on one or more processors employing any of a variety of operating systems or platforms. Furthermore, such software may be written using any of a variety of suitable programming languages ​​and / or programming or scripting tools, and may also be compiled into executable machine language code or intermediate code that is executed on a framework or virtual machine. Generally, in various implementations, the functionality of the program modules may be combined or distributed as desired.

[0180] Furthermore, embodiments of the present invention may be implemented as methods, examples of which have been provided. The actions performed as part of a method may be sequenced in any suitable manner. Thus, although shown as sequential actions in the illustrated embodiments, embodiments may be constructed in which the actions are performed in a different order than shown, including performing some actions simultaneously.

[0181] Although the present invention has been described by way of examples of preferred embodiments, it will be appreciated that various other adjustments and modifications may be made within the spirit and scope of the present invention. It is therefore intended that the appended claims encompass all such changes and modifications within the true spirit and scope of the present invention.

Claims

1. A system for controlling a vehicle, the system comprising: an input interface configured to accept a current state of the vehicle, an image of an environment proximate to the current state of the vehicle, and a destination of the vehicle; a memory configured to store a probabilistic motion planner and an adaptive predictive controller, wherein the probabilistic motion planner is configured to receive a current state of the vehicle, a destination of the vehicle, and an image of the environment to generate a sequence of parametric probability distributions about a sequence of target states defining a motion plan for the vehicle, wherein parameters of each parametric probability distribution define a first-order moment and at least one higher-order moment of the probability distribution, wherein the adaptive predictive controller is configured to optimize a cost function over a prediction horizon to generate a sequence of control commands for one or more actuators of the vehicle, wherein the optimization of the cost function balances tracking costs of different state variables in the sequence of target states defined by the first-order moment, and wherein one or more of the higher-order moments of the probability distribution are used to weight the different state variables in the balancing of the tracking costs; a processor configured to execute the probabilistic motion planner by submitting the vehicle's current state, the vehicle's destination, and an image of the environment to the probabilistic motion planner, and to execute the adaptive predictive controller by submitting the parameter probability distribution sequence generated by the probabilistic motion planner to the adaptive predictive controller to generate the control command sequence; and An output interface is configured to output at least one control command determined by the adaptive predictive controller to at least one actuator of the vehicle.

2. The system according to claim 1, wherein: The higher-order moments of the probability distribution indicate a confidence of the probabilistic motion planner regarding the motion plan of the vehicle, wherein the adaptive predictive controller increases the weight of the tracking in the balance optimization as the confidence increases, thereby allowing lower deviations of the predicted vehicle state values ​​from the target state sequence, and decreases the weight of the tracking in the balance optimization as the confidence decreases, thereby allowing larger deviations of the predicted vehicle state values ​​from the target state sequence.

3. The system according to claim 1, wherein: The adaptive predictive controller is executed at a rate greater than the probabilistic motion planner, such that the processor executes the adaptive predictive controller at least once for each execution of the probabilistic motion planner.

4. The system according to claim 3, wherein: The motion plan defines a parameter probability distribution sequence over the target state sequence as a function of time for a time period greater than the prediction horizon.

5. The system according to claim 1, wherein: Each target state of the vehicle includes multiple state variables, so that each parameter probability distribution is a multivariate distribution of parameters defining the distribution of the respective state variables, so that the first-order moment of the parameter probability distribution is the mean, and the higher-order moments of the parameter probability distribution are covariance matrices with values ​​that vary in space and time.

6. The system according to claim 1, wherein: The probabilistic motion planner comprises a particle filter that propagates a set of particles in time to represent a set of likelihoods of the target state at a time instance, such that each particle comprises a Gaussian distribution of values ​​of the target state at the time instance, wherein the first-order moment of the parameter probability distribution at the time instance is a weighted average of the particles and the higher-order moments of the parameter probability distribution at the time instance are weighted covariances of the particles.

7. The system according to claim 1, wherein: The adaptive predictive controller is a model predictive controller (MPC) having the cost function formulated as a time-varying staged least squares cost function comprising a tracking cost weighted by a time-varying positive definite weighting matrix having values ​​derived from the higher-order moments of the probability distribution.

8. The system according to claim 7, wherein: The values ​​of the time-varying positive definite weighting matrix are inversely proportional to the values ​​of a covariance matrix sequence formed by the high-order moments of the probability distribution.

9. The system according to claim 8, wherein: The higher-order moments of the parameter probability distribution are determined individually for each control objective, and the MPC tracking cost is weighted using a time-varying positive definite diagonal weighting matrix, and each of the diagonal values ​​is calculated based on an inversely proportional relationship to each of the corresponding higher-order moments of the parameter probability distribution.

10. The system according to claim 9, wherein: The adaptive predictive controller uses a time-varying positive definite weighting matrix in the tracking cost, which is calculated as the stage-wise scaled inverse of the sequence of covariance matrices of the parameter probability distributions from the probabilistic motion planner.

11. The system according to claim 10, wherein: The inverse relationship between the weighting matrix and the covariance matrix includes a performance-specific scaling that can be relatively different for each of the tracking control targets and a saturation function that bounds each of the time-varying positive definite weighting matrices between a lower bound and an upper bound on the weight of each of the control targets in the tracking cost function.

12. The system according to claim 11, wherein The processor calculates the diagonal values ​​of the weighting matrix using only the diagonal values ​​of the covariance matrix, so that the weighting matrix becomes a diagonal matrix.

13. The system of claim 1, wherein: Optimization of the cost function balances the cost of tracking the sequence of target states defined by the first-order moments of the parameter probability distribution against the cost of at least one other measure of the vehicle's motion, wherein the importance of the tracking cost is weighted using one or more of the higher-order moments of the probability distribution in the balanced optimization.

14. The system according to claim 1, wherein: The adaptive predictive controller is a model predictive controller (MPC) that optimizes a balance cost function using constrained optimization subject to activity constraints selected based on a control objective, wherein the adaptive predictive controller feeds back the activity constraints to the probabilistic motion planner, which is configured to adjust the higher-order moments of the probability distribution based on the type and / or number of the activity constraints.

15. The system of claim 1, wherein: The probabilistic motion planner uses a set of control functions, wherein each control function is configured to transition the current state of the vehicle to a target state based on a corresponding control objective of the control function, wherein at least some of the control functions are probabilistic and include a deterministic component for transitioning the current state to the target state and a probabilistic component for determining a probability distribution of values ​​around the target state determined by the deterministic component, such that an output of the probabilistic control function is a parametric probability distribution about the target state, the parametric probability distribution defining a first-order moment and at least one higher-order moment of the probability distribution.

16. A method for controlling a vehicle, wherein: The method uses a processor coupled to a memory, the memory storing a probabilistic motion planner and an adaptive predictive controller, wherein the probabilistic motion planner is configured to accept a current state of a vehicle, a destination of the vehicle, and an image of an environment to generate a sequence of parametric probability distributions about a sequence of target states defining a motion plan for the vehicle, wherein parameters of each parametric probability distribution define a first-order moment and at least one higher-order moment of the probability distribution, wherein the adaptive predictive controller is configured to optimize a cost function over a prediction horizon to generate a sequence of control commands for one or more actuators of the vehicle, wherein the optimization of the cost function balances tracking costs of different state variables in the sequence of target states defined by the first-order moments, wherein one or more of the higher-order moments of the probability distribution are used to weight the different state variables in the balancing of tracking costs, wherein the processor is coupled to stored instructions for executing the method, wherein the instructions, when executed by the processor, perform the steps of the method, comprising the steps of: receiving a current state of the vehicle, an image of an environment proximate to the current state of the vehicle, and a destination of the vehicle; executing the probabilistic motion planner by submitting to the probabilistic motion planner a current state of the vehicle, a destination of the vehicle, and an image of the environment; executing the adaptive predictive controller by submitting the parameter probability distribution sequence generated by the probabilistic motion planner to the adaptive predictive controller to generate the control command sequence; and At least one control command determined by the adaptive predictive controller is submitted to at least one actuator of the vehicle.

17. The method according to claim 16, wherein Optimization of the cost function balances the cost of tracking the sequence of target states defined by the first-order moments of the parameter probability distribution against the cost of at least one other measure of the vehicle's motion, wherein the importance of the tracking cost is weighted using one or more of the higher-order moments of the probability distribution in the balanced optimization.

18. The method according to claim 16, wherein The adaptive predictive controller is a model predictive controller (MPC) having the cost function formulated as a time-varying staged least squares cost function comprising a tracking cost weighted by a time-varying positive definite weighting matrix having values ​​derived from the higher-order moments of the probability distribution.

19. The method according to claim 18, wherein The values ​​of the time-varying positive definite weighting matrix are inversely proportional to the values ​​of a covariance matrix sequence formed by the high-order moments of the probability distribution.

20. A non-transitory computer-readable storage medium having a program embodied thereon, the program being executable by a processor to perform a method, wherein: The non-transitory computer-readable storage medium stores a probabilistic motion planner and an adaptive predictive controller, wherein the probabilistic motion planner is configured to accept a current state of a vehicle, a destination of the vehicle, and an image of an environment to generate a sequence of parameter probability distributions about a sequence of target states defining a motion plan for the vehicle, wherein parameters of each parameter probability distribution define a first-order moment and at least one higher-order moment of the probability distribution, wherein the adaptive predictive controller is configured to optimize a cost function within a prediction horizon to generate a sequence of control commands for one or more actuators of the vehicle, wherein the optimization of the cost function balances tracking costs of different state variables in the sequence of target states defined by the first-order moment, wherein one or more of the higher-order moments of the probability distribution are used to weight the different state variables in the balancing of the tracking costs, and the method comprises the following steps: receiving a current state of the vehicle, an image of an environment proximate to the current state of the vehicle, and a destination of the vehicle; executing the probabilistic motion planner by submitting to the probabilistic motion planner a current state of the vehicle, a destination of the vehicle, and an image of the environment; executing the adaptive predictive controller by submitting the parameter probability distribution sequence generated by the probabilistic motion planner to the adaptive predictive controller to generate the control command sequence; and At least one control command determined by the adaptive predictive controller is submitted to at least one actuator of the vehicle.

Images