Environmental Envelope Control

Environmental Envelope Control May 26 th, 2014 Stanford University Mechanical Engineering Dept. Dynamic Design Lab Stephen Erlien Avinash Balachandran J. Christian Gerdes

Motivation New technologies are empowering modern vehicles: Sensors that detect surrounding environment BMW Cameras Subaru Velodyne Steer-by-wire to augment driver s steering commands LIDAR Ibeo NISSAN Steer-by-wire How can these be used to make human drivers safer?

Envelope Control Restrict human operator to safe regimes of operation while granting full control to the operator otherwise Used in aircraft control (Wells 2006) Successfully applied to vehicle stability (Beal 2012) Safe Region Bounds dictated by physical limitations of the vehicle Max steady-state yaw rate Rear tire slip angle at peak tire force

Two envelopes: Envelope Control Safe Region

Two envelopes: Envelope Control Safe Region Safe Region

Two envelopes: Envelope Control Safe Region Safe Region Want to predict, and possibly modify, vehicle behavior so that it remains inside these two envelopes (ie, collision free & stable) To do this real-time, would like to formulate this as a convex problem(s)

Vehicle Model Vehicle motion can be described by 2 velocity states yaw rate, r sideslip, β = tan U y U x If we assume... Ux Uy Ux = constant then equations of motion are linear: β = F yf+f yr mu x r r = aa yfbf yr I zz where m is vehicle mass and Izz is vehicle yaw inertia

Vehicle Model Vehicle position relative to a nominal path can be described by 3 position states heading deviation, φ lateral deviation, e distance along path, s If we assume... φ is small then equations of motion are linear: φ = r e = U x sin φ + U y cos φ U x φ + U x β s = U x cos φ U y sin φ U x U x β φ U x Everything thus far is linear

Tire Force Model Brush tire model (Fiala 1954 and Pacejka 2012) Front tires Control actuator is steering Can solve for a desired, F yf and then map it to a steer angle, δ, using this model F yf = f α f = f β + aa U x δ Rear tires Cannot steer rear wheels Cannot use same approach as front tires

Rear Tire Model Near term prediction model Affine approximation of tire curve using measured rear slip angle, α r F yr = F yr C α r α r α r Captures tire saturation behavior but valid for only short periods of time, i.e. < 150 [ms] (Beal 2010) Longer term prediction model Need to consider vehicle behavior beyond 150 [ms] for obstacle avoidance For now, we will assume linear tire model: F yr = C αr α r Model is still linear

Modifying Vehicle Behavior Want vehicle to remain inside the safe envelopes for the foreseeable future (finite prediction horizon) In addition, we want vehicle trajectories that are... 1) Identical to the driver s current steering command 2) Smooth Let us define best trajectory as minimizing objective function: where u driver u 0 + γ u k u k1 2 u is optimal steering trajectory, u driver is driver s command, γ is a tunable parameter, and k is index into predicted future This would be a convex problem except for...

Obstacles Obstacles (in general) present a choice of avoidance on left or right This creates a non-convex set of collision-free trajectories How to address this challenge? Need to pre-process the environment into a form that can be quickly evaluated

Environmental Envelope Key features Easy to generate Enables fast calculation of feasible, collision free trajectories Consists of safe tubes through environment

Environmental Envelope t=10 t=9 t=8 t=7 t=6 Key features Easy to generate Enables fast calculation of feasible, collision free trajectories Consists of safe tubes through environment t=5 t=4 t=3 t=2 t=1*ts

Environmental Envelope Key features Easy to generate Enables fast calculation of feasible, collision free trajectories Consists of safe tubes through environment Each tube defines a convex set of collision free trajectories* * if the trajectories are generated using the vehicle model presented previously

Environmental Envelope Key features Easy to generate Enables fast calculation of feasible, collision free trajectories Consists of safe tubes through environment Each tube defines a convex set of collision free trajectories From this set, a preferred trajectory can be quickly determined

Environmental Envelope t=10 t=9 t=8 t=7 t=6 t=5 t=4 t=3 t=2 t=1*ts Key features Easy to generate Enables fast calculation of feasible, collision free trajectories Consists of safe tubes through environment Each tube defines a convex set of collision free trajectories From this set, a preferred trajectory can be quickly determined

Environmental Envelope Key features Easy to generate Enables fast calculation of feasible, collision free trajectories Consists of safe tubes through environment Each tube defines a convex set of collision free trajectories From this set, a preferred trajectory can be quickly determined

Environmental Envelope Key features Easy to generate Enables fast calculation of feasible, collision free trajectories Consists of safe tubes through environment Each tube defines a convex set of collision free trajectories From this set, a preferred trajectory can be quickly determined From this preferred subset, the best trajectory is easily found

Block Diagram Driver Safe Envelope Controller Vehicle Model Vehicle Envelope Tube Generation Friction Estimation Vehicle State Estimation Obstacle Detection Vehicle Sensors Trajectory Planning (Obstacle Avoidance + Stability) Vehicle (Plant) Motion Prediction and Uncertainty Estimation

Putting It All Together Vehicle and environmental envelopes Safe Region Safe Region

Putting It All Together Vehicle and environmental envelopes

Experimental Results P1 test bed: Drive- and steer-by-wire Vehicle states measured from GPS/INS Assumes obstacle and road position are known Control implementation Real-time hardware for signal processing/low level actuation MPC computation off loaded to ruggedized i7 (only using 1 core) CVXGEN used to generate realtime optimization code Test conditions: Gravel on asphalt parking lot Variable friction surface

Experimental Results

Experimental Results New Stanford testbed, X1 Drive- and steer-by-wire Independent 4 wheel braking Haptic handwheel Vehicle states measured from GPS/INS Assumes obstacle and road position are known Control implementation dspace MicroAutobox for signal processing/low level actuation MPC computation off loaded to ruggedized i7 (only using 1 core) CVXGEN used to generate real-time optimization code Test conditions: Gravel on asphalt parking lot Variable friction surface ISO 3888-1 Double Lane Change

Experimental Results Double lane change @ 35 mph (15.5 m/s) on gravel

Cooperation Between Driver and Controller Steer-by-wire enables a mismatch between the hand wheel and the road wheels Road wheels turned right Hand wheel turned left This is good in that the controller can ensure vehicle safety But bad in that it can feel unnatural to the driver How can we improve communication between the human driver and the vehicle/controller so that they work more as a team?

Incorporating Moving Obstacles t=10 t=9 t=8 t=7 Deterministic moving obstacles Can be incorporated into current framework Tubes represent lateral bounds on vehicle position in both time and space t=6 t=5 t=4 t=3 t=2 t=1*ts

Incorporating Moving Obstacles t=10 t=9 t=8 t=7 t=6 Deterministic moving obstacles Can be incorporated into current framework Tubes represent lateral bounds on vehicle position in both time and space Obstacles with a similar velocity as I appear as elongated static obstacles t=5 t=4 t=3 t=2 t=1*ts

Incorporating Moving Obstacles t=10 t=9 t=8 t=7 t=6 t=5 t=4 t=3 Deterministic moving obstacles Can be incorporated into current framework Tubes represent lateral bounds on vehicle position in both time and space Obstacles with a similar velocity as I appear as elongated static obstacles Obstacles traveling in opposite direction appear as small static obstacle at moment when I pass them t=2 t=1*ts

Incorporating Moving Obstacles Simulation of driver attempting to merge into occupied lane Controller intervenes and avoids collision

Incorporating Moving Obstacles In previous simulation, vehicle was allowed to get very close to moving obstacle In practice, this is not desirable because moving obstacle s motion isn t certain Can account for this uncertainty by virtually increasing the size of the obstacle This increase in size does not have to be uniform, can account for different uncertainties in longitudinal and lateral speed and/or position

Incorporating Moving Obstacles Simulation of driver attempting to merge into occupied lane Controller accounts for uncertainty in moving obstacle s location

Virtual Sizing of Obstacles Increased virtual sizing of obstacle could be a function of... Uncertainty in measured position (i.e., sensor noise) Uncertainty in predicted motion Increased importance/safety hazard (i.e., pedestrians) Speed of my vehicle relative to obstacle Obstacle s virtual size should increase if relative speed is high Virtual size can grow throughout prediction horizon for obstacles like pedestrians 0.1 [s] into prediction horizon 2.0 [s] into prediction horizon More confident in pedestrian s future position over such a short period of time This is mostly sensor noise Less confident in future position further into the prediction horizon. This is mostly uncertainty in pedestrian s motion

Virtual Obstacles In addition to making some obstacles appear larger, can also create completely virtual obstacles to account for sensor blind spots Ignoring possibility of hidden pedestrian behind parked car? Modeling the possibility of a hidden pedestrian as a virtual obstacle If too much uncertainty, virtual obstacles and virtual obstacle sizes could fully block road In these cases, brake actuation is required to avoid collision?

Brake Actuation in Envelope Control Sometimes steering alone cannot ensure vehicle remains within safe envelopes, brake actuation is needed 1. Pending violation of safe driving envelopes 2. Blocked tube Simplified approach to brake + steer: 1. Identify situations where current speed is too high for obstacle avoidance + vehicle stability 2. Solve for desired, slower Ux profile 3. Compute required Fx profile to achieve desired Ux profile 4. Use (slightly modified) environmental envelope framework to compute δ Required modifications to environmental envelope De-rate Fy capabilities of tires to accommodate Fx profile Modify affine vehicle model to account for desired Ux profile Now with the ability to brake, the controller can navigate more complicated situations which arise when there is significant uncertainty

Variable Speed and Brake Actuation Simulation with possibly hidden pedestrian and uncertain oncoming traffic with possibly hidden additional traffic The idea for this situation came from Susumu at Nissan Moving vehicle occlusion #2 parked vehicle occlusion #1 Environmental Envelope approach: Virtual Sizing of Moving Obstacle Virtual Moving Obstacle Static Obstacle Virtual Static Obstacle

Variable Speed and Brake Actuation Simulation with possibly hidden pedestrian and uncertain oncoming traffic with possibly hidden additional traffic