-------------------------------------------------------------------------------- -- -- AVC controller -- -- Author(s) : Willem Hagemann -- -- fomc --flowpipe --noonion -- --configStringFlowpipe "max_steps=10000; step_width=1/10; \ -- result_denominator=4096;" -- --configStringFomc "flowpipeGCPreimage=false;" -------------------------------------------------------------------------------- DECL bool flow ; [0,2] int avc_mode ; const [0,2] int normal = 0; const [0,2] int hi_acc = 1; const [0,2] int lo_acc = 2; ---- Parameter for drive controler const real a_max = 3.0; //maximum allowed acceleration const real a_min = -6.0; //minimum allowed acceleration const real v_max = 40.0; const real v_min = 0.0; ---- Parameter for dynamics const real a = 0.29; const real b = 0.01; const real dist = 50.0; //desired distance ---- ego car ---- -- Global constraints for velocity [v_min, v_max] real v_initial; [v_min - 10.0, v_max + 50.0] real v; ---- ahead car ---- -- Global constraints for velocity [v_min, v_max] real v_ahead_initial; ---- Distance ---- [ -10.0, 500.0] real d_initial; [ -10.0, 500.0] real d; [ 0.0, 500.0 ] real t; INIT flow = true; v = v_initial; d = d_initial; t = 0.0; ( avc_mode = hi_acc and a*v_ahead_initial - a*v + b*d - b*dist >= a_max - 0.1 ) or ( avc_mode = normal and a_min <= a*v_ahead_initial - a*v + b*d - b*dist and a*v_ahead_initial - a*v + b*d - b*dist <= a_max ) or ( avc_mode = lo_acc and a*v_ahead_initial - a*v + b*d - b*dist <= a_min + 0.1 ); INVAR JUMP avc_mode = hi_acc and a*v_ahead_initial - a*v + b*d - b*dist <= a_max - 0.1 -> flow' = false; avc_mode = normal and a*v_ahead_initial - a*v + b*d - b*dist >= a_max -> flow' = false ; avc_mode = normal and a*v_ahead_initial - a*v + b*d - b*dist <= a_min -> flow' = false ; avc_mode = lo_acc and a*v_ahead_initial - a*v + b*d - b*dist >= a_min + 0.1 -> flow' = false ; a*v_ahead_initial - a*v + b*d - b*dist >= a_max -> avc_mode' = hi_acc ; a_min < a*v_ahead_initial - a*v + b*d - b*dist and a*v_ahead_initial - a*v + b*d - b*dist < a_max -> avc_mode' = normal ; a*v_ahead_initial - a*v + b*d - b*dist <= a_min -> avc_mode' = lo_acc ; FLOW avc_mode = normal -> ( d/dt(v) = a*v_ahead_initial - a*v + b*d - b*dist and d/dt(d) = v_ahead_initial - v ); avc_mode = hi_acc -> ( d/dt(v) = a_max and d/dt(d) = v_ahead_initial - v ); avc_mode = lo_acc -> ( d/dt(v) = a_min and d/dt(d) = v_ahead_initial - v ); -- constants true -> d/dt(d_initial) = 0.0; true -> d/dt(v_initial) = 0.0; true -> d/dt(v_ahead_initial) = //disturb of 0.1 is safe, 0.25 is unsafe 0.0 ; true -> d/dt(t) = 1.0; TARGET ( d_initial >= 450.0 and d_initial <= 500.0 and v_initial >= v_min and v_initial <= v_max ) -> ( v_ahead_initial >= v_min and v_ahead_initial <= v_max -> d >= 0.0 ) and ( v_ahead_initial >= v_min and v_ahead_initial <= 0.8*v_max and t >= 160.0 -> dist - 5.0 <= d and d <= dist + 5.0 and v >= -1.0 and v <= v_max + 8.0 ) ;