PaperlayerView exact revision on arXiv

Source-linked AI summary

Stochastic Model Predictive Control with a Safety Guarantee for Automated Driving: Extended Version

Tim Brüdigam, Michael Olbrich, Dirk Wollherr, Marion Leibold

arXiv:2009.09381v5eess.SYcs.SY

TL;DR

Uncertain motion by surrounding vehicles makes safe, efficient automated-vehicle planning difficult: SMPC plans efficiently but allows constraint violations. The paper combines SMPC with a fail-safe backup that gates whether its first input is applied. It proves recursive feasibility and reports simulation benefits compared with pure SMPC and pure FTP.

  • Problem

    SMPC supports efficient planning under uncertainty but permits a probability of constraint violation, while robust approaches can be conservative.

    Source: §I · p. 1

  • Method

    SMPC+FTP plans an optimistic SMPC trajectory and applies its first input only if a fail-safe backup trajectory remains possible.

    Source: §I · p. 1

  • Results

    The method is proved recursively feasible, and simulations compare its benefits with pure SMPC and pure FTP.

    Source: B. Recursive Feasibility · p. 7, §VIII · p. 18

  • Takeaways & Limitations

    The proposed method combines efficient SMPC planning with a fail-safe trajectory planning problem that ensures only safe-state-preserving SMPC inputs are applied.

    Source: §VIII · p. 18

    In the simulations, traffic-vehicle behavior was deterministic even though the ego vehicle modeled it probabilistically.

    Source: §VII · p. 17

Abstract

from arXiv · show

Automated vehicles require efficient and safe planning to maneuver in uncertain environments. Largely this uncertainty is caused by other traffic participants, e.g., surrounding vehicles. Future motion of surrounding vehicles is often difficult to predict. Whereas robust control approaches achieve safe, yet conservative motion planning for automated vehicles, Stochastic Model Predictive Control (SMPC) provides efficient planning in the presence of uncertainty. Probabilistic constraints are applied to ensure that the maximal risk remains below a predefined level. However, safety cannot be ensured as probabilistic constraints may be violated, which is not acceptable for automated vehicles. Here, we propose an efficient trajectory planning framework with safety guarantees for automated vehicles. SMPC is applied to obtain efficient vehicle trajectories for a finite horizon. Based on the first optimized SMPC input, a guaranteed safe backup trajectory is planned using reachable sets. This backup is used to overwrite the SMPC input if necessary for safety. Recursive feasibility of the safe SMPC algorithm is proved. Highway simulations show the effectiveness of the proposed method regarding performance and safety.

I. INTRODUCTION

SMPC can plan efficiently amid uncertainty but permits constraint violations, while fail-safe trajectory planning (FTP) offers safety with potentially conservative motion. SMPC+FTP combines them by applying an SMPC input only when a safe backup remains feasible.

I. INTRODUCTION

  • SMPC+FTP applies the first optimized SMPC input only when a fail-safe backup trajectory remains possible, combining efficient planning with a safety guarantee.The backup is planned at every time step based on the first optimized SMPC input.Source: §I · p. 1
  • The method is proved recursively feasible: if a solution exists at one time step, a solution also exists at the next.The proof assumes surrounding traffic vehicles follow the underlying traffic rules; it does not require specific traffic rules.Source: §I · p. 2

A. Related Work

  • Prior work spans POMDP and reinforcement-learning methods, MPC for vehicle and maneuver planning, and FTP based on worst-case reachable sets.MPC's iterative replanning with constraints addresses a changing environment; FTP uses reachable sets to support formal safety guarantees.Source: A. Related Work · p. 2

B. Notation

  • The notation section distinguishes scalars, vectors, and matrices and defines the weighted quadratic form ∥z∥Z = z⊤Zz.It also defines event probability as Pr(.) and uses xh for the state at time step h.Source: B. Notation · p. 3

II. PRELIMINARIES

  • MPC solves a finite-horizon constrained optimal control problem, applies only the first optimized input, then resolves the problem at the next time step.The paper distinguishes the current time step h from prediction steps k within the optimization problem.Source: §II · p. 3

A. SMPC with Chance Constraints

  • SMPC uses chance constraints to represent uncertain safety requirements, trading conservatism against risk; for β < 1, some constraint-violation probability is allowed.The paper considers uncertainty in surrounding vehicles and describes reformulating the chance constraint into a deterministic expression for a solver.Source: A. SMPC with Chance Constraints · p. 3

B. Fail-safe Trajectory Planning

Fail-safe trajectory planning uses worst-case predictions and hard safety constraints, with a terminal state that remains safe beyond the prediction horizon. The vehicle models provide the predictions and uncertainty propagation used by the MPC problems.

B. Fail-safe Trajectory Planning

  • FTP uses reachability-based hard safety constraints against worst-case vehicle motion and requires a terminal state that permits safe motion beyond the horizon.The FTP problem considers worst-case uncertainty realizations and is related to invariant-set computation in robust MPC.Source: B. Fail-safe Trajectory Planning · p. 3

A. Ego Vehicle Model

  • The paper uses a kinematic bicycle model for the ego vehicle, linearized around its current state and discretized for finite-horizon MPC.The state comprises longitudinal and lateral position, orientation, and velocity; the input comprises acceleration and steering angle.Source: A. Ego Vehicle Model · p. 4
  • The ego-vehicle model also constrains inputs and states, including acceleration, steering, road boundaries, and nonnegative velocity.The admissible-state and input sets include road and velocity constraints.Source: A. Ego Vehicle Model · p. 4

B. Target Vehicle Model

  • Target vehicles use a linear, discrete-time point-mass prediction model that propagates uncertainty; the paper notes that other linear prediction models could also be used.The model includes input disturbances and sensor noise, with sensor noise assumed to lie in a compact, convex, bounded set.Source: B. Target Vehicle Model · pp. 4–5

IV. STOCHASTIC MODEL PREDICTIVE CONTROL WITH SAFETY GUARANTEE

SMPC+FTP combines efficient, uncertainty-aware planning with fail-safe planning to address the limitations of using either alone. Its safety framework defines safe states and input sequences and is designed to preserve recursive feasibility.

A. SMPC+FTP Method

  • A safe ego-vehicle state requires no lateral motion and either a speed below the target vehicle ahead or zero speed; a safe input sequence avoids collisions and eventually stops the vehicle.The safe state is defined for an ego vehicle fully within one lane.Source: A. SMPC+FTP Method · p. 5
  • The safety argument assumes target vehicles follow the modeled traffic rules and cannot decelerate more strongly than the ego vehicle.Under these assumptions, deceleration with zero steering can bring the ego vehicle to rest in its current lane.Source: A. SMPC+FTP Method · p. 5
  • SMPC+FTP solves an SMPC and an FTP optimal control problem at each step, applying the first SMPC input only when a fail-safe trajectory can be found.The method is designed to guarantee recursive feasibility: a solution at one time step guarantees a solution at the next.Source: A. SMPC+FTP Method · p. 5, §IV · p. 5

1) SMPC:

The method uses chance-constrained SMPC for efficient planning and checks whether its first input admits a fail-safe continuation. When needed, it falls back to FTP or a still-valid safe input sequence.

1) SMPC:

  • SMPC plans an efficient, optimistic trajectory using chance-constrained collision avoidance, which permits a small collision probability set by the risk parameter.The constraints use probabilistic predictions of target vehicles rather than requiring avoidance in worst-case scenarios.Source: 1) SMPC · p. 5

2) FTP:

  • FTP plans against worst-case target-vehicle predictions and requires collision avoidance plus a safe terminal state.The terminal state must satisfy the safe-state definition after the full FTP input sequence.Source: 2) FTP · p. 5

3) Infeasible FTP:

  • If FTP is infeasible, the controller applies the first input of the still-valid safe sequence, shifting that sequence if infeasibility continues.The four cases distinguish whether SMPC and FTP are feasible; when FTP is feasible, the first SMPC input is used if SMPC is feasible, otherwise the first FTP input is used.Source: 3) Infeasible FTP · p. 6, 4) Summary of SMPC+FTP · p. 6

4) Summary of SMPC+FTP:

  • In regular cases, the efficient SMPC input is applied after an FTP check, while FTP and the maintained safe sequence provide a safety fallback, including for rare events.The FTP OCP is based on the first SMPC input; if it is infeasible, the previously obtained safe sequence remains valid.Source: 4) Summary of SMPC+FTP · pp. 6–7, 3) Infeasible FTP · p. 6

B. Recursive Feasibility

The method establishes recursive feasibility under stated system and initial-safe-sequence assumptions, with safety conditional on surrounding vehicles following the underlying traffic rules. Its SMPC planning uses deterministic target-vehicle predictions and safety rectangles that account for vehicle dimensions and braking-distance differences.

B. Recursive Feasibility

  • Under Assumptions 3 and 4, SMPC+FTP maintains a feasible safe trajectory at successive time steps, provided surrounding target vehicles follow the underlying traffic rules.The proof does not require specific traffic rules, but its safety and recursive-feasibility guarantee depends on adherence to the assumed rules.Source: B. Recursive Feasibility · p. 7

1) Deterministic Target Vehicle Prediction:

  • For SMPC, target vehicles continue their current maneuver over the prediction horizon, with lane changes predicted only when shape and lateral motion indicate movement toward an adjacent lane.The velocity reference is the current target-vehicle velocity, and the lateral reference changes to the adjacent lane center when both lane-change conditions hold.Source: 1) Deterministic Target Vehicle… · p. 7

2) Target Vehicle Safety Area:

  • The target-vehicle safety rectangle keeps vehicle centers apart by at least vehicle length and width, with an added longitudinal margin for braking-distance differences.The braking-distance margin assumes zero reaction time and similar maximum deceleration for the ego and target vehicles.Source: 2) Target Vehicle Safety Area · pp. 7–8

3) Chance Constraint Reformulation:

The chance constraint is replaced by a deterministic safety constraint by enlarging the target-vehicle rectangle to cover prediction uncertainty at a chosen risk level. A confidence ellipse for the position error is over-approximated by a rectangle for constraint generation.

3) Chance Constraint Reformulation:

  • A larger β produces a larger target-vehicle safety area and reduces risk.The chance constraint is not solved directly; a deterministic approximation is used instead.Source: 3) Chance Constraint Reformulation · p. 8
  • The risk parameter β sets the desired probability that the true target-vehicle position lies within an uncertainty region around its predicted mean.The reduced error covers longitudinal and lateral position, and its confidence-region tolerance is obtained from a chi-squared distribution with two degrees of freedom.Source: 3) Chance Constraint Reformulation · pp. 8–9
  • The confidence ellipse is over-approximated by a rectangle so its uncertainty can enlarge the safety area used for linear SMPC constraints.The ellipse semi-major and semi-minor axes increase the rectangle parameters.Source: 3) Chance Constraint Reformulation · p. 9

4) SMPC Constraint Generation:

SMPC generates linear collision-avoidance constraints for target vehicles according to their positions relative to the ego vehicle. Cases use the initial vehicle configuration to select constraints while retaining linearity.

4) SMPC Constraint Generation:

  • Constraint cases are selected from the initial vehicle configuration, using predicted target-vehicle positions but the initial ego state to keep constraints linear.The overview is not complete; the full case requirements and constraint parameters are given in Appendix D.Source: 4) SMPC Constraint Generation · p. 9
  • No constraint is generated when the target vehicle is farther away than the long-range threshold; at closer distances, the cases depend on separation and relative lane positions.For distances beyond the close-range threshold, vertical constraints apply behind or in front of the target vehicle.Source: 4) SMPC Constraint Generation · p. 9
  • When vehicles are in the same lane or the target vehicle is in the adjacent lane to the left, a bounded inclined constraint can permit the ego vehicle to switch left.If the target vehicle is to the right, a horizontal constraint is used at its safety rectangle.Source: 4) SMPC Constraint Generation · p. 10
  • If the ego vehicle is ahead of a target vehicle in the same lane, no constraint is generated because the target vehicle is assumed to keep its distance.The selected cases formulate safety constraints in the SMPC optimal control problem.Source: 4) SMPC Constraint Generation · p. 10

5) SMPC Optimal Control Problem:

The SMPC problem becomes a quadratic program with linear constraints, while the backup FTP planner uses worst-case reachable sets and a safe invariant terminal set. Target-vehicle occupancy predictions incorporate traffic rules and account for motion between discrete prediction steps.

5) SMPC Optimal Control Problem:

  • The SMPC optimal control problem is a quadratic program with linear constraints, using a deterministic reformulation of the chance constraint to account for uncertainty.Its major linear-constraint calculations are performed before optimization.Source: 5) SMPC Optimal Control Problem · p. 10

B. Failsafe Trajectory Planning

1) Target Vehicle Occupancy Prediction:

  • FTP reachable sets use minimal and maximal target-vehicle inputs while respecting traffic rules, then take a rectangular convex hull across consecutive steps to cover continuous-time motion.The reachable set is enlarged for the ego-vehicle shape and used to keep the ego-vehicle center outside the occupied region.Source: 1) Target Vehicle Occupancy Prediction · pp. 10–11
  • For a target vehicle behind the ego vehicle in the same lane, three placeholder sets cover collision-avoiding motion in that lane and possible lane changes to either adjacent lane.The occupancy prediction assumes only a single lane change within the prediction horizon.Source: 1) Target Vehicle Occupancy Prediction · pp. 10–11, Figure 5 · p. 11

2) FTP Constraint Generation:

FTP generates constraints by classifying the ego and target vehicles’ relative positions, with its cases designed around finding a safe terminal state. Its constraints are more conservative than SMPC’s, in part because FTP does not actively plan overtaking maneuvers.

2) FTP Constraint Generation:

  • FTP constraint cases distinguish relative vehicle positions, with matching case letters marking similarities to SMPC and asterisks denoting FTP cases.Figure 6 illustrates selected cases; lighter colors indicate cases equal to SMPC constraint-generation cases.Source: 2) FTP Constraint Generation · p. 11, Figure 6 · p. 11
  • When the EV and TV are sufficiently far apart, FTP generates no constraint; when the EV follows a nearby TV in the same lane, it places a vertical constraint behind that TV.For the same-lane case, FTP does not actively plan an overtake.Source: 2) FTP Constraint Generation · p. 11
  • For an EV on a different lane from a TV, FTP generates horizontal constraints at the TV regardless of their relative longitudinal position.The text identifies cases F* and H* for this configuration.Source: 2) FTP Constraint Generation · p. 11
  • For a TV behind the EV, FTP may model up to three placeholder predictions for lane changes or overtaking, using vertical, horizontal, or inclined constraints depending on distance and lane.Nearby predictions use vertical constraints in the EV lane and horizontal constraints in adjacent lanes; farther predictions use inclined constraints in neighboring lanes.Source: 2) FTP Constraint Generation · pp. 11–12
  • FTP constraints are more conservative overall because its trajectory must end in a safe state, making planned overtaking difficult to incentivize.Source: 2) FTP Constraint Generation · p. 12

3) Safe Invariant Terminal Set:

The FTP problem uses a safe invariant terminal set to maintain collision avoidance beyond its finite horizon, and its optimization accounts for worst-case target-vehicle uncertainty. The simulations evaluate regular and emergency highway scenarios with surrounding vehicles.

3) Safe Invariant Terminal Set:

  • The FTP terminal state requires road-aligned orientation and enough distance from a vehicle ahead for maximal EV braking to avoid collision if both vehicles brake maximally.The terminal distance and velocity conditions ensure this safety property beyond the prediction horizon while retaining linear constraints.Source: 3) Safe Invariant Terminal Set · p. 12

4) FTP Optimal Control Problem:

  • The FTP optimal control problem replaces SMPC’s probabilistic constraint with a constraint for worst-case target-vehicle uncertainty and remains a quadratic program with linear constraints.The resulting problem can be solved efficiently.Source: 4) FTP Optimal Control Problem · p. 12

VI. RESULTS

A. Simulation Setup

  • The setup places the ego vehicle on a three-lane highway with five surrounding target vehicles, aiming for safe and efficient maneuvering near a reference speed.In the emergency scenario, TV5 brakes, TV4 moves to avoid it, and TV1 brakes softly; TV4 later moves left to pass TV2.Source: A. Simulation Setup · pp. 12–13

B. Regular Highway Scenario

In the regular highway scenario, SMPC+FTP produces efficient lane-changing behavior, and its reported overall cost is far below FTP’s. Risk-parameter variations yield nearly similar vehicle behavior, with smoother inputs at higher risk parameters.

1) SMPC+FTP:

  • In the regular highway scenario, the EV changes lanes to the center with steering angle δ < 0.04, then moves left and passes TV2; orientation remains φ < 0.11 while reference velocity is maintained with negligible acceleration inputs.Average solve times are 0.11 s for SMPC and 0.15 s for FTP.Source: 1) SMPC+FTP · pp. 13–14

2) Comparison to SMPC and FTP:

  • Both SMPC and FTP problems remain feasible throughout the regular simulation, so SMPC inputs are always applied and SMPC alone would produce the same result.Source: 2) Comparison to SMPC and FTP · p. 14
  • FTP alone does not overtake in this scenario because its more conservative constraints keep the EV in its current lane.FTP lane changes are described as rare and used to avoid obstacles rather than actively overtake.Source: 2) Comparison to SMPC and FTP · p. 14, Figure 11 · p. 15
  • 11.32 versus 4.03e4 in overall cost, SMPC+FTP yields more efficient behavior than FTP in the regular scenario.The paper attributes the efficiency to keeping velocity close to the reference velocity.Source: 2) Comparison to SMPC and FTP · p. 15

3) Risk Parameter Variation:

  • Across five risk-parameter examples, EV behavior is almost similar, while a larger risk parameter can reduce cost and slightly increases conservatism while smoothing inputs.Table II presents the overall simulation costs for the analyzed risk parameters.Source: 3) Risk Parameter Variation · p. 15, Table II · p. 15

4) Varying Simulation Settings:

Across the tested highway scenarios, SMPC+FTP combines efficient SMPC planning with a fail-safe backup, maintaining safety even when SMPC becomes infeasible. The discussion identifies horizon and constraint-generation choices that bound performance and motion quality.

4) Varying Simulation Settings:

  • In 1000 simulations of 125 steps each, SMPC+FTP handled every run without collisions.Initial vehicle positions and velocities were randomly selected, with five surrounding vehicles.Source: 4) Varying Simulation Settings · p. 15

1) SMPC:

  • SMPC alone collides with TV4 after rare events make its chance-constrained problem infeasible, despite the collision probability initially remaining below the risk parameter.The emergency sequence is illustrated in Figure 12.Source: 1) SMPC · p. 15, Figure 12 · p. 16

2) SMPC+FTP:

  • When SMPC becomes infeasible, FTP supplies inputs; when FTP cannot find a new backup, the previous safe sequence is applied until a lane change is safe.The emergency motion is less smooth, but safety is guaranteed in this scenario.Source: 2) SMPC+FTP · p. 16, Figure 14 · p. 16
  • Jsim = 3.34e4 for SMPC+FTP versus Jsim = 4.28e4 for pure FTP; the combined method safely passes slower vehicles when possible.Pure FTP remains safe in the emergency scenario but does not overtake TV1 and TV2.Source: 2) SMPC+FTP · p. 16

VII. DISCUSSION

  • A relatively short SMPC horizon reduces the chance of infeasibility, while an FTP horizon that is too short can abort lane changes before they occur.FTP uses a terminal state with a specified distance to vehicles ahead and zero road-relative orientation offset.Source: §VII · p. 17
  • Alternating SMPC and FTP inputs can oscillate; less aggressive SMPC constraints avoided this behavior in the simulations.Emergency inputs also produced relatively high steering angles, which could be improved by defining more constraint-generation cases.Source: §VII · p. 17

VIII. CONCLUSION

  • The combined method’s properties are not restricted to the specific SMPC and FTP planners used, but efficiency depends on constraint generation, which must be adapted for urban traffic.The conclusion also identifies safety-critical transportation and non-transportation applications as possible extensions.Source: §VII · p. 17, §VIII · p. 18
  • SMPC plans optimistic trajectories, while FTP ensures that only inputs preserving a safe state are applied.The simulation study compares the method with pure SMPC and pure FTP.Source: §VIII · p. 18

APPENDIX A LINEARIZED AND DISCRETIZED SYSTEM MATRICES

The appendix gives the linearized, time-discrete system matrices and proves recursive feasibility by induction. The proof maintains a safe trajectory at each step by refreshing or reusing safe input sequences.

APPENDIX A LINEARIZED AND DISCRETIZED SYSTEM MATRICES

  • The appendix provides the linearized, time-discrete system matrices Ad and Bd from equation (6).Their entries are displayed in the supplied matrix expressions.Source: Appendix A · p. 18

APPENDIX B PROOF OF THEOREM 1

  • Recursive feasibility is established by induction: nonempty feasible sets Γ_h imply nonempty Γ_{h+1} for every h.The proof concludes that the proposed method is safe and recursively feasible.Source: Appendix B · p. 18
  • A feasible FTP problem produces a new safe input set, while an infeasible one leaves the previous safe set valid after shifting.The induction begins from an initially safe trajectory under Assumption 4.Source: Appendix B · p. 18

APPENDIX C PROOF OF LEMMA 1

The appendix proves that the longitudinal position error density depends only on its position variance, with the same reasoning applying laterally. It also details constraint-generation cases for SMPC and FTP, including velocity-dependent following distance and lane-change conditions.

APPENDIX C PROOF OF LEMMA 1

  • The longitudinal position error density depends only on σ²_x,k, independently of longitudinal velocity variance and position-velocity covariance; the same proof applies laterally.The proof marginalizes over velocity and identifies the resulting position-coordinate distribution as univariate Gaussian.Source: Appendix C · p. 19

APPENDIX D CONSTRAINT GENERATION

  • The closing-distance function includes a velocity-difference term, so larger relative velocity increases the required distance between the EV and TV.The appendix also specifies EV-corner positions and distinguishes cases where the EV center is outside a TV lane but its shape overlaps it.Source: Appendix D · p. 19
  • FTP constraint cases include placeholder predictions for surrounding vehicles, with case slopes bounded and limiting cases applied when those bounds are exceeded.For SMPC, left-lane overtaking is planned only when the EV is faster than the TV; a behind-TV vertical constraint replaces the inclined constraint when the EV is too close.Source: Appendix D · p. 19
Loading 2009.09381v5…